Lesson 7 - Multithreading, doing things parallel

Introduction

Last lesson, we have heard of multitasking and multithreading, but we have not seen anything of it. This will be changed now. We create a second subclass of EV3 and name it Jukebox. This class plays tones and music, later we will add some light effects. In this lesson we use it as a playground for multithreading.

Then we will look at different aspects of multithreading. We will write little programs and we will get familiar with it.

Class Jukebox

We code a second subclass of EV3 with a design, that realizes the qualities, we formulated at the end of the last lesson:

#!/usr/bin/env python3

import ev3, time

TRIAS = {
    "tempo": 80,
    "tones": [
        ["c'", 1],
        ["e'", 1],
        ["g'", 1],
        ["c''",3],
    ]
}

HAPPY_BIRTHDAY = {
    "tempo": 100,
    "tones": [
        ["d'", 0.75],
        ["d'", 0.25],
        ["e'", 1],
        ["d'", 1],
        ["g'", 1],
        ["f#'", 2],
        ["d'", 0.75],
        ["d'", 0.25],
        ["e'", 1],
        ["d'", 1],
        ["a'", 1],
        ["g'", 2],
        ["d'", 0.75],
        ["d'", 0.25],
        ["d''", 1],
        ["b'", 1],
        ["g'", 1],
        ["f#'", 1],
        ["e'", 1],
        ["c''", 0.75],
        ["c''", 0.25],
        ["b'", 1],
        ["g'", 1],
        ["a'", 1],
        ["g'", 2]
    ]
}

class Jukebox(ev3.EV3):
    def __init__(self, protocol: str=None, host: str=None, ev3_obj=None):
        super().__init__(protocol=protocol, host=host, ev3_obj=ev3_obj)
        self._volume = 1
        self._temperament = 440
        self._pos_tone = None
        self._plays = False

    @property
    def volume(self):
        return self._volume
    @volume.setter
    def volume(self, value:int):
        self._volume = value

    @property
    def temperament(self):
        return self._temperament
    @temperament.setter
    def temperament(self, value:float):
        self._temperament = value

    def play_tone(self, tone: str, duration: float=0) -> None:
        volume = self._volume
        if tone == "p":
            self.stop()
            return
        elif tone.startswith("c"):
            freq = self._temperament * 2**(-9/12)
        elif tone.startswith("d"):
            freq = self._temperament * 2**(-7/12)
        elif tone.startswith("e"):
            freq = self._temperament * 2**(-5/12)
        elif tone.startswith("f"):
            freq = self._temperament * 2**(-4/12)
        elif tone.startswith("g"):
            freq = self._temperament * 2**(-2/12)
        elif tone.startswith("a"):
            freq = self._temperament
        elif tone.startswith("b"):
            freq = self._temperament * 2**(2/12)
        else:
            raise AttributeError('unknown Tone: ' + tone)

        if len(tone) > 1:
            if tone[1] == "#":
                freq *= 2**(1/12)
            elif tone[1] == "b":
                freq /= 2**(1/12)

        if tone.endswith("'''"):
            freq *= 4
        elif tone.endswith("''"):
            freq *= 2
        elif tone.endswith("'"):
            pass
        else:
            freq /= 2
        ops = b''.join([
            ev3.opSound,
            ev3.TONE,
            ev3.LCX(volume),
            ev3.LCX(round(freq)),
            ev3.LCX(round(1000*duration))
        ])
        self.send_direct_cmd(ops)

    def stop(self) -> None:
        self.send_direct_cmd(ev3.opSound + ev3.BREAK)
        self._plays = False

    def _init_tone(self) -> None:
        self._pos_tone = 0
        self._plays = True

    def _next_tone(self, song) -> float:
        if self._pos_tone == len(song["tones"]):
            return -1
        tone, beats = song["tones"][self._pos_tone]
        self.play_tone(tone)
        self._pos_tone += 1
        return 60 * beats / song["tempo"]

    def play_song(self, song:dict) -> None:
        self._init_tone()
        while self._plays:
            duration = self._next_tone(song)
            if duration == -1:
                break
            time.sleep(duration)
        if self._plays:
            self._plays = False
            self.stop()
      

Remarks:

• You already know all the operations from lesson 2.
• The frequencies of the tones are calculated in the 12 tone equal temperament and then rounded to integers.
• The object attribute _plays is a flag, that signals, that actually a song is played. This class reacts correctly, if method stop is called, while a song is played. It stops playing.
• Method play_song is time consuming but does not block the EV3 device. From this point of view, it behaves like methods drive_straight, drive_turn, rotate_to and drive_to of class TwoWheelVehicle.
• The songs are defined as JSON objects, which is like a poor mans midi notation.
• As all our classes, Jukebox is a layer of abstraction. This one encapsulates the playing of music.

The documentation of module ev3_sound:

Help on module ev3_sound:

NAME
    ev3_sound

CLASSES
    ev3.EV3(builtins.object)
        Jukebox
    
    class Jukebox(ev3.EV3)
     |  plays tones and songs
     |  
     |  Method resolution order:
     |      Jukebox
     |      ev3.EV3
     |      builtins.object
     |  
     |  Methods defined here:
     |  
     |  __init__(self, protocol:str=None, host:str=None, ev3_obj=None)
     |      Establish a connection to a LEGO EV3 device
     |      
     |      Keyword Arguments (either protocol and host or ev3_obj):
     |      protocol: None, 'Bluetooth', 'Usb' or 'Wifi'
     |      host: None or mac-address of the LEGO EV3 (f.i. '00:16:53:42:2B:99')
     |      ev3_obj: None or an existing EV3 object (its connections will be used)
     |  
     |  play_song(self, song:dict) -> None
     |      plays a song
     |      
     |      example:
     |      jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
     |      jukebox.play_song(ev3_sound.HAPPY_BIRTHDAY)
     |  
     |  play_tone(self, tone:str, duration:float=0) -> None
     |      plays a tone
     |      
     |      Attributes:
     |      tone: name of tone f.i. "c'", "cb''", "c#"
     |      
     |      Keyword Attributes:
     |      duration: length (sec.) of the tone (value 0 means forever)
     |  
     |  stop(self) -> None
     |      stops the sound
     |  
     |  ----------------------------------------------------------------------
     |  Data descriptors defined here:
     |  
     |  temperament
     |      temperament of the tones (delfault: 440 Hz)
     |  
     |  volume
     |      volume of sound [0 - 100] (default: 1)
     |  
     |  ----------------------------------------------------------------------
     |  Methods inherited from ev3.EV3:
     |  
     |  __del__(self)
     |      closes the connection to the LEGO EV3
     |  
     |  send_direct_cmd(self, ops:bytes, local_mem:int=0, global_mem:int=0) -> bytes
     |      Send a direct command to the LEGO EV3
     |      
     |      Arguments:
     |      ops: holds netto data only (operations), the following fields are added:
     |        length: 2 bytes, little endian
     |        counter: 2 bytes, little endian
     |        type: 1 byte, DIRECT_COMMAND_REPLY or DIRECT_COMMAND_NO_REPLY
     |        header: 2 bytes, holds sizes of local and global memory
     |      
     |      Keyword Arguments:
     |      local_mem: size of the local memory
     |      global_mem: size of the global memory
     |      
     |      Returns: 
     |        sync_mode is STD: reply (if global_mem > 0) or message counter
     |        sync_mode is ASYNC: message counter
     |        sync_mode is SYNC: reply of the LEGO EV3
     |  
     |  wait_for_reply(self, counter:bytes) -> bytes
     |      Ask the LEGO EV3 for a reply and wait until it is received
     |      
     |      Arguments:
     |      counter: is the message counter of the corresponding send_direct_cmd
     |      
     |      Returns:
     |      reply to the direct command
     |  
     |  ----------------------------------------------------------------------
     |  Data descriptors inherited from ev3.EV3:
     |  
     |  __dict__
     |      dictionary for instance variables (if defined)
     |  
     |  __weakref__
     |      list of weak references to the object (if defined)
     |  
     |  sync_mode
     |      sync mode (standard, asynchronous, synchronous)
     |      
     |      STD:   Use DIRECT_COMMAND_REPLY if global_mem > 0,
     |             wait for reply if there is one.
     |      ASYNC: Use DIRECT_COMMAND_REPLY if global_mem > 0,
     |             never wait for reply (it's the task of the calling program).
     |      SYNC:  Always use DIRECT_COMMAND_REPLY and wait for reply.
     |      
     |      The general idea is:
     |      ASYNC: Interruption or EV3 device queues direct commands,
     |             control directly comes back.
     |      SYNC:  EV3 device is blocked until direct command is finished,
     |             control comes back, when direct command is finished.               
     |      STD:   NO_REPLY like ASYNC with interruption or EV3 queuing,
     |             REPLY like SYNC, synchronicity of program and EV3 device.
     |  
     |  verbosity
     |      level of verbosity (prints on stdout).

DATA
    HAPPY_BIRTHDAY = {'tempo': 100, 'tones': [["d'", 0.75], ["d'", 0.25], ...
    TRIAS = {'tempo': 80, 'tones': [["c'", 1], ["e'", 1], ["g'", 1], ["c''"...
      

We write this little program to test it:

#!/usr/bin/env python3

import ev3, ev3_sound

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1
jukebox.play_song(ev3_sound.HAPPY_BIRTHDAY)
    

Its output:

20:17:14.956954 Sent 0x|0C:00|2A:00|80|00:00|94:01:01:82:26:01:00|
20:17:15.408330 Sent 0x|0C:00|2B:00|80|00:00|94:01:01:82:26:01:00|
20:17:15.559332 Sent 0x|0C:00|2C:00|80|00:00|94:01:01:82:4A:01:00|
20:17:16.160783 Sent 0x|0C:00|2D:00|80|00:00|94:01:01:82:26:01:00|
20:17:16.762240 Sent 0x|0C:00|2E:00|80|00:00|94:01:01:82:88:01:00|
20:17:17.363682 Sent 0x|0C:00|2F:00|80|00:00|94:01:01:82:72:01:00|
20:17:18.565853 Sent 0x|0C:00|30:00|80|00:00|94:01:01:82:26:01:00|
20:17:19.017158 Sent 0x|0C:00|31:00|80|00:00|94:01:01:82:26:01:00|
20:17:19.168137 Sent 0x|0C:00|32:00|80|00:00|94:01:01:82:4A:01:00|
20:17:19.769569 Sent 0x|0C:00|33:00|80|00:00|94:01:01:82:26:01:00|
20:17:20.371090 Sent 0x|0C:00|34:00|80|00:00|94:01:01:82:B8:01:00|
20:17:20.972644 Sent 0x|0C:00|35:00|80|00:00|94:01:01:82:88:01:00|
20:17:22.174288 Sent 0x|0C:00|36:00|80|00:00|94:01:01:82:26:01:00|
20:17:22.625924 Sent 0x|0C:00|37:00|80|00:00|94:01:01:82:26:01:00|
20:17:22.777000 Sent 0x|0C:00|38:00|80|00:00|94:01:01:82:4B:02:00|
20:17:23.378498 Sent 0x|0C:00|39:00|80|00:00|94:01:01:82:EE:01:00|
20:17:23.980124 Sent 0x|0C:00|3A:00|80|00:00|94:01:01:82:88:01:00|
20:17:24.581646 Sent 0x|0C:00|3B:00|80|00:00|94:01:01:82:72:01:00|
20:17:25.183178 Sent 0x|0C:00|3C:00|80|00:00|94:01:01:82:4A:01:00|
20:17:25.784707 Sent 0x|0C:00|3D:00|80|00:00|94:01:01:82:0B:02:00|
20:17:26.236062 Sent 0x|0C:00|3E:00|80|00:00|94:01:01:82:0B:02:00|
20:17:26.387118 Sent 0x|0C:00|3F:00|80|00:00|94:01:01:82:EE:01:00|
20:17:26.988627 Sent 0x|0C:00|40:00|80|00:00|94:01:01:82:88:01:00|
20:17:27.590142 Sent 0x|0C:00|41:00|80|00:00|94:01:01:82:B8:01:00|
20:17:28.191678 Sent 0x|0C:00|42:00|80|00:00|94:01:01:82:88:01:00|
20:17:29.393731 Sent 0x|07:00|43:00|80|00:00|94:00|

Combining multiple tasks without multithreading

Whe combine driving and playing a song:

#!/usr/bin/env python3

import ev3, ev3_sound, ev3_vehicle

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,                 # radius_wheel
    0.1175,                  # tread
    ev3_obj=jukebox
)
vehicle.drive_turn(25, 0.2)
jukebox.play_song(ev3_sound.HAPPY_BIRTHDAY)
vehicle.stop()

This program does two independent things, it plays a song, which uses EV3's sound resource and it drives the vehicle, which uses two motors. Two independent actions and no need of multithreading, why that? This is a combination of actions with different character:

• When calling drive_turn without setting an angle, this is an unlimited action and control comes back directly.
• play_song does the timing. Control comes back when the song is finished.
• The call of method stop ends the unlimited movement by interruption. Control directly comes back.

This says, we can split our action in three parts:

• Immediate return:

  
  vehicle.drive_turn(25, 0.2)
   

• Time consuming:

  
  jukebox.play_song(ev3_sound.HAPPY_BIRTHDAY)
   

• Immediate return:

  
  vehicle.stop()
   

The timing is done by the time consuming parts of an action! Please take a look at the program The depressed giraffe of lesson 6 and identify the time consuming actions.

Let's come to a first conclusion. Executing multiple tasks does not necessarily need multithreading. Often it's possible to combine the actions in a sequence so that the correct timing is given and all tasks are done as desired. But this needs a clear understanding of the time consumption and the dependencies. The result is a sequence of actions, some of them are time consuming, others return immediately and are grouped around the time consuming actions, which do the timing.

This does not allow to run time consuming actions parallel! The program always waits until control is back.

Multithreading

Now we use multithreading to execute two independent actions. First we add some information to song HAPY_BIRTHDAY:

HAPPY_BIRTHDAY = {
    "tempo": 100,
    "beats_per_bar": 3,
    "upbeat": 1,
    "led_sequence": [ev3.LED_ORANGE, ev3.LED_GREEN, ev3.LED_RED, ev3.LED_GREEN],
    "tones": [
        ...

Then we run this program:

#!/usr/bin/env python3

import ev3, ev3_sound, threading, time

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1

def change_color(led_pattern: bytes) -> None:
    ops = b''.join([
        ev3.opUI_Write,
        ev3.LED,
        led_pattern
    ])
    jukebox.send_direct_cmd(ops)

def colors(song: dict) -> None:
    if "upbeat" in song:
        time.sleep(60 * song["upbeat"] / song["tempo"])
    pos_led = 0
    while plays:
        change_color(song["led_sequence"][pos_led])
        pos_led += 1
        pos_led %= len(song["led_sequence"])
        time.sleep(60 * song["beats_per_bar"] / song["tempo"])
    change_color(ev3.LED_GREEN)

plays = True  
threading.Thread(
    target=colors,
    args=(ev3_sound.HAPPY_BIRTHDAY,)
).start()
jukebox.play_song(ev3_sound.HAPPY_BIRTHDAY)
plays = False

      

Remarks:

• Both, function colors and method play_song are time consuming. Both run parallel, but play_song does the timing, it stops colors when the song is finished.
• Class Thread allows to run any callable in its own thread. This says, control immediately comes back.
• We call colors with an argument. If you want its return value, this needs some more logic, method start does not return it. The execution is asynchronous and handling return values is one of the drawbacks. We are lucky, there is no return value.
• Please consult the documentation of module threading for details.

This programs output:

20:15:20.846839 Sent 0x|0C:00|2A:00|80|00:00|94:01:01:82:26:01:00|
20:15:21.298260 Sent 0x|0C:00|2B:00|80|00:00|94:01:01:82:26:01:00|
20:15:21.447336 Sent 0x|08:00|2C:00|80|00:00|82:1B:03|
20:15:21.449398 Sent 0x|0C:00|2D:00|80|00:00|94:01:01:82:4A:01:00|
20:15:22.050950 Sent 0x|0C:00|2E:00|80|00:00|94:01:01:82:26:01:00|
20:15:22.652346 Sent 0x|0C:00|2F:00|80|00:00|94:01:01:82:88:01:00|
20:15:23.249905 Sent 0x|08:00|30:00|80|00:00|82:1B:01|
20:15:23.253805 Sent 0x|0C:00|31:00|80|00:00|94:01:01:82:72:01:00|
20:15:24.455873 Sent 0x|0C:00|32:00|80|00:00|94:01:01:82:26:01:00|
20:15:24.907189 Sent 0x|0C:00|33:00|80|00:00|94:01:01:82:26:01:00|
20:15:25.052517 Sent 0x|08:00|34:00|80|00:00|82:1B:02|
20:15:25.058203 Sent 0x|0C:00|35:00|80|00:00|94:01:01:82:4A:01:00|
20:15:25.659172 Sent 0x|0C:00|36:00|80|00:00|94:01:01:82:26:01:00|
20:15:26.260541 Sent 0x|0C:00|37:00|80|00:00|94:01:01:82:B8:01:00|
20:15:26.855334 Sent 0x|08:00|38:00|80|00:00|82:1B:01|
20:15:26.862090 Sent 0x|0C:00|39:00|80|00:00|94:01:01:82:88:01:00|
20:15:28.064249 Sent 0x|0C:00|3A:00|80|00:00|94:01:01:82:26:01:00|
20:15:28.515525 Sent 0x|0C:00|3B:00|80|00:00|94:01:01:82:26:01:00|
20:15:28.657901 Sent 0x|08:00|3C:00|80|00:00|82:1B:03|
20:15:28.666590 Sent 0x|0C:00|3D:00|80|00:00|94:01:01:82:4B:02:00|
20:15:29.268142 Sent 0x|0C:00|3E:00|80|00:00|94:01:01:82:EE:01:00|
20:15:29.869902 Sent 0x|0C:00|3F:00|80|00:00|94:01:01:82:88:01:00|
20:15:30.460544 Sent 0x|08:00|40:00|80|00:00|82:1B:01|
20:15:30.471457 Sent 0x|0C:00|41:00|80|00:00|94:01:01:82:72:01:00|
20:15:31.072930 Sent 0x|0C:00|42:00|80|00:00|94:01:01:82:4A:01:00|
20:15:31.674404 Sent 0x|0C:00|43:00|80|00:00|94:01:01:82:0B:02:00|
20:15:32.125746 Sent 0x|0C:00|44:00|80|00:00|94:01:01:82:0B:02:00|
20:15:32.263208 Sent 0x|08:00|45:00|80|00:00|82:1B:02|
20:15:32.276589 Sent 0x|0C:00|46:00|80|00:00|94:01:01:82:EE:01:00|
20:15:32.878185 Sent 0x|0C:00|47:00|80|00:00|94:01:01:82:88:01:00|
20:15:33.479785 Sent 0x|0C:00|48:00|80|00:00|94:01:01:82:B8:01:00|
20:15:34.065854 Sent 0x|08:00|49:00|80|00:00|82:1B:01|
20:15:34.081116 Sent 0x|0C:00|4A:00|80|00:00|94:01:01:82:88:01:00|
20:15:35.282992 Sent 0x|07:00|4B:00|80|00:00|94:00|
20:15:35.868486 Sent 0x|08:00|4C:00|80|00:00|82:1B:01|
      

Both of them run independently but are thought to work synchronized. We take a closer look to the synchronization and realize, that the synchronization becomes worse. If we played a longer song, we could see and hear the growing time shift. Every command needs some time to execute. And these small durations add up. There are more tones than color changes, this makes that tones fall behind colors.

Class Jukebox with colors

We add colors to class Jukebox. This needs two more methods, change_color and _colors:

    def change_color(self, led_pattern: bytes) -> None:
        ops = b''.join([
            ev3.opUI_Write,
            ev3.LED,
            led_pattern
        ])
        self.send_direct_cmd(ops)

    def _colors(self, song: dict) -> None:
        if "upbeat" in song:
            time.sleep(60 * song["upbeat"] / song["tempo"])
        pos_led = 0
        while self._plays:
            self.change_color(song["led_sequence"][pos_led])
            pos_led += 1
            pos_led %= len(song["led_sequence"])
            time.sleep(60 * song["beats_per_bar"] / song["tempo"])
        self.change_color(ev3.LED_GREEN)
    

We modify method play_song:

    def play_song(self, song:dict) -> None:
        self._init_tone()
        threading.Thread(
            target=self._colors,
            args=(song,)
        ).start()
        while self._plays:
            duration = self._next_tone(song)
            if duration == -1:
                break
            time.sleep(duration)
        if self._plays:
            self._plays = False
            self.stop()
      

We test it:

#!/usr/bin/env python3

import ev3, ev3_sound

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.play_song(ev3_sound.HAPPY_BIRTHDAY)
      

We test the stopping:

#!/usr/bin/env python3

import ev3, ev3_sound, time, threading

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
threading.Thread(
    target=jukebox.play_song,
    args=(ev3_sound.HAPPY_BIRTHDAY,)
).start()
time.sleep(5)
jukebox.stop()

This program runs three threads. One changes the colors, one the tones and the base thread stops them.

Exact timing

For a better timing we improve class Jukebox and modify method _colors:

    def _colors(self, song: dict) -> None:
        time_action = time.time()
        if "upbeat" in song:
            time_action += 60 * song["upbeat"] / song["tempo"]
            gap = time_action - time.time()
            if gap > 0:
                time.sleep(gap)
        pos_led = 0
        while self._plays:
            self.change_color(song["led_sequence"][pos_led])
            pos_led += 1
            pos_led %= len(song["led_sequence"])
            time_action += 60 * song["beats_per_bar"] / song["tempo"]
            gap = time_action - time.time()
            if gap > 0:
                time.sleep(gap)
        self.change_color(ev3.LED_GREEN)
      

and method play_song:

    def play_song(self, song:dict) -> None:
        self._init_tone()
        threading.Thread(
            target=self._colors,
            args=(song,)
        ).start()
        time_action = time.time()
        while self._plays:
            duration = self._next_tone(song)
            if duration == -1:
                break
            time_action += duration
            gap = time_action - time.time()
            if gap > 0:
                time.sleep(gap)
        if self._plays:
            self._plays = False
            self.stop()
      

we test it with this program:

#!/usr/bin/env python3

import ev3, ev3_sound

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1
jukebox.play_song(ev3_sound.HAPPY_BIRTHDAY)
      

the output:

09:39:22.873872 Sent 0x|0C:00|2A:00|80|00:00|94:01:01:82:26:01:00|
09:39:23.324479 Sent 0x|0C:00|2B:00|80|00:00|94:01:01:82:26:01:00|
09:39:23.474193 Sent 0x|0C:00|2C:00|80|00:00|94:01:01:82:4A:01:00|
09:39:23.474699 Sent 0x|08:00|2D:00|80|00:00|82:1B:03|
09:39:24.074629 Sent 0x|0C:00|2E:00|80|00:00|94:01:01:82:26:01:00|
09:39:24.674645 Sent 0x|0C:00|2F:00|80|00:00|94:01:01:82:88:01:00|
09:39:25.274656 Sent 0x|0C:00|30:00|80|00:00|94:01:01:82:72:01:00|
09:39:25.275207 Sent 0x|08:00|31:00|80|00:00|82:1B:01|
09:39:26.475232 Sent 0x|0C:00|32:00|80|00:00|94:01:01:82:26:01:00|
09:39:26.924492 Sent 0x|0C:00|33:00|80|00:00|94:01:01:82:26:01:00|
09:39:27.074212 Sent 0x|0C:00|34:00|80|00:00|94:01:01:82:4A:01:00|
09:39:27.074697 Sent 0x|08:00|35:00|80|00:00|82:1B:02|
09:39:27.674645 Sent 0x|0C:00|36:00|80|00:00|94:01:01:82:26:01:00|
09:39:28.274532 Sent 0x|0C:00|37:00|80|00:00|94:01:01:82:B8:01:00|
09:39:28.874644 Sent 0x|0C:00|38:00|80|00:00|94:01:01:82:88:01:00|
09:39:28.875532 Sent 0x|08:00|39:00|80|00:00|82:1B:01|
09:39:30.075237 Sent 0x|0C:00|3A:00|80|00:00|94:01:01:82:26:01:00|
09:39:30.524485 Sent 0x|0C:00|3B:00|80|00:00|94:01:01:82:26:01:00|
09:39:30.674126 Sent 0x|0C:00|3C:00|80|00:00|94:01:01:82:4B:02:00|
09:39:30.674611 Sent 0x|08:00|3D:00|80|00:00|82:1B:03|
09:39:31.274652 Sent 0x|0C:00|3E:00|80|00:00|94:01:01:82:EE:01:00|
09:39:31.874653 Sent 0x|0C:00|3F:00|80|00:00|94:01:01:82:88:01:00|
09:39:32.474641 Sent 0x|0C:00|40:00|80|00:00|94:01:01:82:72:01:00|
09:39:32.475321 Sent 0x|08:00|41:00|80|00:00|82:1B:01|
09:39:33.074650 Sent 0x|0C:00|42:00|80|00:00|94:01:01:82:4A:01:00|
09:39:33.674640 Sent 0x|0C:00|43:00|80|00:00|94:01:01:82:0B:02:00|
09:39:34.124485 Sent 0x|0C:00|44:00|80|00:00|94:01:01:82:0B:02:00|
09:39:34.274186 Sent 0x|0C:00|45:00|80|00:00|94:01:01:82:EE:01:00|
09:39:34.274928 Sent 0x|08:00|46:00|80|00:00|82:1B:02|
09:39:34.874565 Sent 0x|0C:00|47:00|80|00:00|94:01:01:82:88:01:00|
09:39:35.474633 Sent 0x|0C:00|48:00|80|00:00|94:01:01:82:B8:01:00|
09:39:36.074662 Sent 0x|0C:00|49:00|80|00:00|94:01:01:82:88:01:00|
09:39:36.075217 Sent 0x|08:00|4A:00|80|00:00|82:1B:01|
09:39:37.275170 Sent 0x|07:00|4B:00|80|00:00|94:00|
09:39:37.875666 Sent 0x|08:00|4C:00|80|00:00|82:1B:01|
      

This solved the problem! We changed from netto to brutto timing. Now the time distances include the time for execution.

Communication between threads

The base thread and all threads it starts, use the same global data. This allows communication. Every thread can read data of another one. The communication is asynchronous, which is no problem, we are used to asynchronous communication. Think of mails or emails. We already have seen it working. Attribute _plays was used for communication between different threads. If one thread sets this flag, the others read it and react.

Locking

The communication between threads sometimes shows unexpected results. Let's look at a snippet of code, that runs in its own thread. This thread uses a global variable state to tell its actual state to the rest of the world:

STATE_TO_STOP = "TO_STOP"
STATE_STOPPED = "STOPPED"
STATE_FINISHED = "FINISHED"

def finished_or_stopped():
    global state
    if state == STATE_TO_STOP:
        state = STATE_STOPPED
    if state == STATE_STOPPED and not next:
        state = STATE_FINISHED

state = STATE_TO_STOP
next = False
finished_or_stopped()
      

We expect, that there are only two combinations of state and next:

• Before the function is called: state == STATE_TO_STOP and next == False
• After the call of the function: state == STATE_FINISHED and next == False

The experience will show, this is correct in about 99.9999 % of all situations, but not really for all. It may happen, that a foreign thread asks about variable state just after it was changed to value STATE_STOPPED. This seldom case shows a new combination:

• While the function is executed: state == STATE_STOPPED and next == False

The locking mechanism prevents this. We change our function to:

def finished_or_stopped():
    global state
    lock.acquire()
    if state == STATE_TO_STOP:
        state = STATE_STOPPED
    if state == STATE_STOPPED and not next:
        state = STATE_FINISHED
    lock.release()
      

The lock object was created by lock = threading.Lock() and the foreign thread, which asks about the state also must know and use it:

    lock.acquire()
    if state == STATE_STOPPED:
        print("This never happens")
    lock.release()
      

The lock object guaranties, that a second call of method acquire will wait until method release was called (maybe by anyone else). This says either function finished_or_stopped has to wait until the foreign thread has finished its if statement or the foreign thread has to wait until the change of valiable state in function finished_or_stopped is done.

Locking is a very common thechnique. Databases use it to prevent concurrent updates of the same data. Operating systems use locking to manage the usage of hardware resources and so on. But it's never fun to code it. If one forgets a single call of method release, the resource is blocked forever. The good news is, that encapsulation allows to do all this behind the scene. This says, the execution (f.i. usage of the resources) is done through well defined methods, which implement the locking mechanism.

Error handling

Another drawback of asynchronous processing is error handling. If an error is thrown inside a thread, this will not reach the other threads (and not the base thread). Let' look at an example:

#!/usr/bin/env python3

import ev3, ev3_sound, time, threading

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1

def tone(intervall) -> None:
    time_action = time.time()
    for i in range(4):
        jukebox.play_tone("c", 0.1)
        time.sleep(0.5)
        jukebox.play_tone("c'", 0.05)
        time.sleep(0.5)
        jukebox.play_tone("c'", 0.05)
        time.sleep(0.5)
        jukebox.play_tone("c'", 0.05)
        time_action += intervall
        gap = time_action - time.time()
        time.sleep(max(0, gap))
    jukebox.stop()

def led(intervall) -> None:
    time_action = time.time()
    for i in range(2):
        jukebox.change_color(ev3.LED_RED)
        time.sleep(2)
        raise Exception('Something happened')
        jukebox.change_color(ev3.LED_GREEN)
        time_action += intervall
        gap = time_action - time.time()
        time.sleep(max(0, gap))

threading.Thread(target=led, args=(4,)).start()
tone(2)
      

its output:

08:40:18.292905 Sent 0x|08:00|2A:00|80|00:00|82:1B:02|
08:40:18.293778 Sent 0x|0D:00|2B:00|80|00:00|94:01:01:82:83:00:81:64|
08:40:18.795806 Sent 0x|0D:00|2C:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:19.297941 Sent 0x|0D:00|2D:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:19.800143 Sent 0x|0D:00|2E:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:20.294214 Sent 0x|0D:00|2F:00|80|00:00|94:01:01:82:83:00:81:64|
Exception in thread Thread-1:
Traceback (most recent call last):
  File "/usr/lib/python3.4/threading.py", line 920, in _bootstrap_inner
    self.run()
  File "/usr/lib/python3.4/threading.py", line 868, in run
    self._target(*self._args, **self._kwargs)
  File "./test_01.py", line 28, in led
    raise Exception('Something happened')
Exception: Something happend

08:40:20.796906 Sent 0x|0D:00|30:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:21.299067 Sent 0x|0D:00|31:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:21.801280 Sent 0x|0D:00|32:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:22.294128 Sent 0x|0D:00|33:00|80|00:00|94:01:01:82:83:00:81:64|
08:40:22.796186 Sent 0x|0D:00|34:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:23.298252 Sent 0x|0D:00|35:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:23.800432 Sent 0x|0D:00|36:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:24.294131 Sent 0x|0D:00|37:00|80|00:00|94:01:01:82:83:00:81:64|
08:40:24.796197 Sent 0x|0D:00|38:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:25.298273 Sent 0x|0D:00|39:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:25.800424 Sent 0x|0D:00|3A:00|80|00:00|94:01:01:82:06:01:81:32|
08:40:26.294006 Sent 0x|07:00|3B:00|80|00:00|94:00|
      

The exception was handled inside Thread-1. The base thread did not recognize the exception. Maybe this is what we want, maybe not. I prefer a hard stop of everything as the default reaction. Error handling is communication, we add a variable error:

#!/usr/bin/env python3

import ev3, ev3_sound, time, threading, traceback, sys

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1
error = False

def tone(intervall) -> None:
    global error
    time_action = time.time()
    for i in range(4):
        if error:
            sys.exit()
        jukebox.play_tone("c", 0.1)
        time.sleep(0.5)
        jukebox.play_tone("c'", 0.05)
        time.sleep(0.5)
        jukebox.play_tone("c'", 0.05)
        time.sleep(0.5)
        jukebox.play_tone("c'", 0.05)
        time_action += intervall
        gap = time_action - time.time()
        time.sleep(max(0, gap))
    jukebox.stop()

def led(intervall) -> None:
    global error
    try:
        time_action = time.time()
        for i in range(2):
            jukebox.change_color(ev3.LED_RED)
            time.sleep(2)
            raise Exception('Something happened')
            jukebox.change_color(ev3.LED_GREEN)
            time_action += intervall
            gap = time_action - time.time()
            time.sleep(max(0, gap))
    except Exception:
        error = True
        raise

threading.Thread(target=led, args=(4,)).start()
tone(2)
      

the output:

09:05:21.509790 Sent 0x|08:00|2A:00|80|00:00|82:1B:02|
09:05:21.510744 Sent 0x|0D:00|2B:00|80|00:00|94:01:01:82:83:00:81:64|
09:05:22.012858 Sent 0x|0D:00|2C:00|80|00:00|94:01:01:82:06:01:81:32|
09:05:22.515052 Sent 0x|0D:00|2D:00|80|00:00|94:01:01:82:06:01:81:32|
09:05:23.017140 Sent 0x|0D:00|2E:00|80|00:00|94:01:01:82:06:01:81:32|
09:05:23.511133 Sent 0x|0D:00|2F:00|80|00:00|94:01:01:82:83:00:81:64|
Exception in thread Thread-1:
Traceback (most recent call last):
  File "/usr/lib/python3.4/threading.py", line 920, in _bootstrap_inner
    self.run()
  File "/usr/lib/python3.4/threading.py", line 868, in run
    self._target(*self._args, **self._kwargs)
  File "./test_01.py", line 31, in led
    raise Exception('Something happened')
Exception: Something happened

09:05:24.013019 Sent 0x|0D:00|30:00|80|00:00|94:01:01:82:06:01:81:32|
09:05:24.516006 Sent 0x|0D:00|31:00|80|00:00|94:01:01:82:06:01:81:32|
09:05:25.018296 Sent 0x|0D:00|32:00|80|00:00|94:01:01:82:06:01:81:32|
      

Multiplying the code in function tone would prevent the last three beats. Fact is, that function tone has to ask if an error occured, there is no automatic mechanism. This is asynchronous communication.

Coordinating actions of unknown duration

Sometimes, you have a number of parallel actions, but there is no clear responsibility for the timing. You want your program to wait until all of the actions are finished. This also can be solved with multithreading. A Thread object has a join method, which waits until the thread is finished:

#!/usr/bin/env python3

import ev3, ev3_sound, time, datetime, threading

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1

def tone() -> None:
    global jukebox
    jukebox.play_tone("c'", 0.3)
    time.sleep(1)

def led() -> None:
    global jukebox
    jukebox.change_color(ev3.LED_RED)
    time.sleep(2)
    jukebox.change_color(ev3.LED_GREEN)

t1 = threading.Thread(target=tone)
t2 = threading.Thread(target=led)
t1.start()
t2.start()
t1.join()
t2.join()
now = datetime.datetime.now().strftime('%H:%M:%S.%f')
print(now, "all done")
      

The output:

11:19:37.801899 Sent 0x|0E:00|2A:00|80|00:00|94:01:01:82:06:01:82:2C:01|
11:19:37.803362 Sent 0x|08:00|2B:00|80|00:00|82:1B:02|
11:19:39.808145 Sent 0x|08:00|2C:00|80|00:00|82:1B:01|
11:19:39.808878 all done
      

Events

Events allow that one thread signals an event and other threads wait on it. Here is an example:

#!/usr/bin/env python3

import ev3, ev3_sound, threading, time

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1

lock = threading.Lock()

def task1():
    jukebox.play_tone("c", 0.1)
    lock.acquire()
    lock.release()
    jukebox.play_tone("c", 0.1)
    
def task2():
    jukebox.change_color(ev3.LED_RED)
    lock.acquire()
    lock.release()
    jukebox.change_color(ev3.LED_GREEN)

lock.acquire()
threading.Thread(target=task1).start()
threading.Thread(target=task2).start()
time.sleep(5)
lock.release()
      

The base thread and the threads of task1 and task2 all use the same Lock object lock. The event is signaled by the base threads call lock.release(). This allows both tasks to continue their work.

This is a very common case and the above presented code is hard to read. It is a common practice to use class Event instead, which is syntactic sugar, but reads easier. We change the program:

#!/usr/bin/env python3

import ev3, ev3_sound, threading, time

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1

event = threading.Event()

def task1():
    jukebox.play_tone("c", 0.1)
    event.wait()
    jukebox.play_tone("c", 0.1)
    
def task2():
    jukebox.change_color(ev3.LED_RED)
    event.wait()
    jukebox.change_color(ev3.LED_GREEN)

threading.Thread(target=task1).start()
threading.Thread(target=task2).start()
time.sleep(5)
event.set()
      

Both versions produce the same output:

09:47:08.032849 Sent 0x|0D:00|2A:00|80|00:00|94:01:01:82:83:00:81:64|
09:47:08.034327 Sent 0x|08:00|2B:00|80|00:00|82:1B:02|
09:47:13.040731 Sent 0x|0D:00|2C:00|80|00:00|94:01:01:82:83:00:81:64|
09:47:13.041566 Sent 0x|08:00|2D:00|80|00:00|82:1B:01|
      

Timers

There is a special subclass of Thread, that allows to start a thread after some waiting time. Here an example:

#!/usr/bin/env python3

import ev3, ev3_sound, threading, time

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1

def task():
    jukebox.play_tone("c", 0.1)

task()
threading.Timer(5, task).start()
      

The ouput:

09:56:16.543277 Sent 0x|0D:00|2A:00|80|00:00|94:01:01:82:83:00:81:64|
09:56:21.545055 Sent 0x|0D:00|2B:00|80|00:00|94:01:01:82:83:00:81:64|
      

When a Timer is still waiting, it can be canceled:

#!/usr/bin/env python3

import ev3, ev3_sound, threading, time

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.verbosity = 1

def task():
    jukebox.play_tone("c", 0.1)

task()
timer = threading.Timer(5, task)
timer.start()
time.sleep(2)
timer.cancel()
      

Its output:

09:59:56.989692 Sent 0x|0D:00|2A:00|80|00:00|94:01:01:82:83:00:81:64|
      

The second call of task never took place, because the Timer was cancelled while it was waiting.

Interruptable Sleeping

We use class Condition to code an interruptable sleeper:

#!/usr/bin/env python3

import threading, time, datetime
lock = threading.Lock()
cond = threading.Condition(lock)

def task():
    lock.acquire()
    now = datetime.datetime.now().strftime('%H:%M:%S.%f')
    print(now, "task started")
    cond.wait(3)
    now = datetime.datetime.now().strftime('%H:%M:%S.%f')
    print(now, "task ended")
    lock.release()
    

threading.Thread(target=task).start()
time.sleep(1)
lock.acquire()
now = datetime.datetime.now().strftime('%H:%M:%S.%f')
print(now, "notify task1")
cond.notify()
time.sleep(1)
now = datetime.datetime.now().strftime('%H:%M:%S.%f')
print(now, "lock will be released")
lock.release()
      

The output:

10:10:32.073769 task started
10:10:33.075199 notify task1
10:10:34.076750 lock will be released
10:10:34.077305 task ended
      

Remarks:

• The base thread interrupts the sleeping of function task, which runs in its own thread. This is possible because both threads use the same Condition object.
• Every condition is bound to a lock.
• Method wait implicitly releases the lock, which allows another thread to acquire it.
• The base thread calls method notify which wakes up the waiting thread task.
• When notified, task tries to acquire the lock.
• Our example, where the base thread holds the lock for another sec. after notifying task is unusual but demonstrates the role of the lock.
• If no notification takes place, cond.wait(3) waits for three sec. and meanwhile releases the lock.

Modify class EV3

It needs some modifications of class EV3 to prepare it for parallel execution of multiple tasks.

Locking

For the moment, the message counter is the only class attribute. It is the common identity of a direct command and its reply. If we use multiple instances of class EV3 parallel, we need a locking mechanism when changing class attributes.

class EV3:
    _msg_cnt = 41
    _lock = threading.Lock()
      

and we modify method _complete_direct_cmd:

    def _complete_direct_cmd(self, ops:bytes,
                             local_mem:int,
                             global_mem:int) -> bytes:
        if global_mem > 0  or self._sync_mode == SYNC:
            cmd_type = _DIRECT_COMMAND_REPLY
        else:
            cmd_type = _DIRECT_COMMAND_NO_REPLY
        self._lock.acquire()
        if self._msg_cnt < 65535:
            self._msg_cnt += 1
        else:
            self._msg_cnt = 1
        msg_cnt = self._msg_cnt
        self._lock.release()
        return b''.join([
            struct.pack('<hh', len(ops) + 5, msg_cnt),
            cmd_type,
            struct.pack('<h', local_mem * 1024 + global_mem),
            ops
        ])
 

This guaranties, that the message counters are distinct until they are reused after 65.535 direct commands.

Foreign replies

Sometimes it may happen, that two direct commands (both with reply) do not hold the sequence: send cmd_1, receive reply_1, send cmd_2, receive reply_2. Instead we see the sequence: send cmd_1, send cmd_2, receive reply_1, receive reply_2. We don't prevent that because we have independent parallel tasks and we want the communication as fast as possible. This says task_1 may send cmd_1, but get reply_2:

12:15:23.903970 Sent 0x|15:00|1C:02|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
12:15:23.910924 Sent 0x|0E:00|1D:02|00|04:00|99:1C:00:00:81:21:00:01:60|
12:15:23.953569 Recv 0x|0B:00|1C:02|02|98:70:00:00:4A:78:00:00|
12:15:23.954865 Recv 0x|07:00|1D:02|02|0D:00:00:00|
      

We solve this problem with a dictionary of foreign replies and add another class attribute to class EV3:

class EV3:
    _msg_cnt = 41
    _lock = threading.Lock()
    _foreign = {}
      

We add two protected methods:

    def _put_foreign_reply(self, counter: bytes, reply: bytes) -> None:
        if counter in self._foreign:
            raise ValueError('reply with counter ' + counter + ' already exists')
        else:
            self._foreign[counter] = reply

    def _get_foreign_reply(self, counter: bytes) -> bytes:
        if counter in self._foreign:
            reply = self._foreign[counter]
            del self._foreign[counter]
            return reply
        else:
            return None
      

The first adds a reply to the dictionary with its counter as key. The second looks, if the dictionary contains a reply with a given key. If so, it returns the reply and deletes it from the dictionary. We add some code to method wait_for_reply:

    def wait_for_reply(self, counter: bytes) -> bytes:
        self._lock.acquire()
        reply = self._get_foreign_reply(counter)
        if reply:
            self._lock.release()
            if reply[4:5] != _DIRECT_REPLY:
                raise DirCmdError(
                    "direct command {:02X}:{:02X} replied error".format(
                        reply[2],
                        reply[3]
                    )
                )
            return reply
        while True:
            if self._protocol in [BLUETOOTH, WIFI]:
                reply = self._socket.recv(1024)
            else:
                reply = bytes(self._device.read(EP_IN, 1024, 0))
            len_data = struct.unpack('<H', reply[:2])[0] + 2
            reply_counter = reply[2:4]
            if self._verbosity >= 1:
                ...
            if counter != reply_counter:
                self._put_foreign_reply(reply_counter, reply[:len_data])
            else:
                self._lock.release()
                if reply[4:5] != _DIRECT_REPLY:
                    raise DirCmdError(
                        "direct command {:02X}:{:02X} replied error".format(
                            reply[2],
                            reply[3]
                        )
                    )
                return reply[:len_data]
      

The logic:

• It first looks, if the reply is already in the dictionary. If so, it does not communicate with the EV3 device.
• If not, it reads reply for reply until it gets the one it looks for. All foreign replies are put into the dictionary.
• The standard situation is an empty dictionary.
• The locking guaranties an exclusive access to the dictionary.

The dancing robot

This lesson ends with a program, that combines three independent actions:

#!/usr/bin/env python3

import ev3, ev3_sound, ev3_vehicle, threading, time

jukebox = ev3_sound.Jukebox(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
jukebox.volume = 5
vehicle = ev3_vehicle.TwoWheelVehicle(0.02128, 0.1175, ev3_obj=jukebox)

def drive(song):
    time_action = time.time()
    if "upbeat" in song:
        duration = 60 * song["upbeat"] / song["tempo"]
        time_action += duration
        gap = time_action - time.time()
        time.sleep(max(0, gap))
    duration = 2 * 60 * song["beats_per_bar"] / song["tempo"]
    while driving:
        vehicle.drive_turn(speed, 0.2)
        time_action += duration
        gap = time_action - time.time()
        time.sleep(max(0, gap))
        if not driving: break
        vehicle.drive_turn(-speed, -0.2)
        time_action += duration
        gap = time_action - time.time()
        time.sleep(max(0, gap))
    vehicle.stop()

song = ev3_sound.HAPPY_BIRTHDAY
speed = 30
driving = True
threading.Thread(
    target=drive,
    args=(song,)
).start()
jukebox.play_song(song)
driving = False
      

These are three parallel actions, tones, colors, movements. All of them have their own timing. The coordination results from the ratios of the timings, which all are determined by the rhythm of the music. Movement changes every second bar, color changes per bar and the tones fit into bars.

Conclusion

This lesson layed the foundations of multitasking. We coded a sublass of EV3: Jukebox, which uses multithreading. We have done a sightseeing tour, that showed us a number of aspects, we need to take into account. We were no passive visitors, no we wrote little programs and got familiar with multitasking and multithreading. We learned, that the control of time is an important aspect.

We modified class EV3 to prepare it for multiple concurrent tasks. This needed a locking mechanism and some common resources.

Next lesson we will realize some tools, that help to organize and handle multiple tasks. Here is a first specification:

• We want to start, stop and continue tasks.
• We want an easy API for repeated and periodic tasks with exact timing.
• We want to organize tasks as chains of tasks.
• We need some help for locking and error handling.

Lesson 6 - The perfect Subclass

Introduction

This is the third lesson (after lessons 3 and 4) where we work on class TwoWheelVehicle We coded some remarkable methods, but I see two deficits:

• Unlimited movements do not update the position and orientation of the vehicle. If any unlimited movement took place, this information becomes useless.
• Methods drive_straight, drive_turn, drive_to and rotate_to block the EV3 device. This says their access is exclusive and any other task has to wait until the movements are finished.

At the end of this lesson we will have solved both deficits. The second of them prevents multitasking. The key to both solutions is reading and using the positions of the vehicles wheels.

Determine the vehicles position and orientation from its wheel positions

Constructor

We add some code to the constructor:

    def __init__(self, radius_wheel: float, tread: float,
                 protocol: str=None,
                 host: str=None,
                 ev3_obj: ev3.EV3=None):
        super().__init__(protocol=protocol,
                         host=host,
                         ev3_obj=ev3_obj)
        self._radius_wheel = radius_wheel
        self._tread = tread
        self._polarity = 1
        self._port_left = ev3.PORT_D
        self._port_right = ev3.PORT_A
        self._orientation = 0.0
        self._pos_x = 0.0
        self._pos_y = 0.0
        self._orig_diff = None
        self._pos = None
        self._turn = None
        self._moves = False
      

Their meaning:

• _orig_diff: original difference of the two motor positions (original means, when the first movement is started).
• _pos: positions of the wheels from the last calculation of position and orientation (_update).
• _turn: The value of turn for the actual movement.
• _moves: Flag if the vehicle actualy moves.

Reading the wheel positions

We write a little helper method, that returns the operations, which read the wheel positions:

    def _ops_pos(self):
        return b''.join([
            ev3.opInput_Device,
            ev3.READY_RAW,
            ev3.LCX(0),                             # LAYER
            ev3.port_motor_input(self._port_left),  # NO
            ev3.LCX(7),                             # TYPE - EV3-Large-Motor
            ev3.LCX(1),                             # MODE - Degree
            ev3.LCX(1),                             # VALUES
            ev3.GVX(0),                             # VALUE1
            ev3.opInput_Device,
            ev3.READY_RAW,
            ev3.LCX(0),                             # LAYER
            ev3.port_motor_input(self._port_right), # NO
            ev3.LCX(7),                             # TYPE - EV3-Large-Motor
            ev3.LCX(0),                             # MODE - Degree
            ev3.LCX(1),                             # VALUES
            ev3.GVX(4)                              # VALUE1
        ])
      

These operations use 8 bytes of global space, which extends the reply from 5 to 13 bytes. The reading of the wheel positions can be combined with any operations:

        reply = self.send_direct_cmd(ops_any + self._ops_pos(), global_mem=8)
        pos = struct.unpack('<ii', reply[5:])
      

pos[0] holds the position of the left wheel, pos[1] holds the right wheel position. ops_any may be any operation. Its only limitation is not to use the same 8 bytes of the global memory.

Updating position and orientation

We code a second helper method that gets the actual wheel positions as input and sets _x, _y and _o to their actual values. We will call it whenever we read the wheel positions:

    def _update(self, pos: list) -> None:
        if self._pos == None:
            self._orig_diff = pos[1] - pos[0]
            self._pos = pos
            return
        step = [self._polarity * (pos[0] - self._pos[0]),
                self._polarity * (pos[1] - self._pos[1])]
        self._pos = pos
        # orientation
        diff = self._pos[1] - self._pos[0] - self._orig_diff
        self._orientation = self._polarity * diff * self._radius_wheel / self._tread
        # location
        if step[0] == 0 and step[1] == 0:
            pass
        elif self._turn == 0 or step[0] == step[1]:
            # straight
            dist = step[0] * 2 * math.pi * self._radius_wheel / 360
            self._pos_x += dist * math.cos(math.radians(self._orientation))
            self._pos_y += dist * math.sin(math.radians(self._orientation))
        else:
            # turn
            if not self._moves:
                radius_turn = 0.5 * self._tread * (step[1] + step[0]) / (step[1] - step[0])
            elif self._turn > 0:
                radius_turn = self._tread * (100 / self._turn - 0.5)
            else:
                radius_turn = self._tread * (100 / self._turn + 0.5)
            angle = (step[1] - step[0]) * self._radius_wheel / self._tread
            angle += 180
            angle %= 360
            angle -= 180
            fact = 2.0 * radius_turn * math.sin(math.radians(0.5*angle))
            self._pos_x += fact * math.cos(math.radians(self._orientation - 0.5*angle))
            self._pos_y += fact * math.sin(math.radians(self._orientation - 0.5*angle))
      

A few remarks:

• The initialization:

  
          if self._pos == None:
              self._orig_diff = pos[1] - pos[0]
              self._pos = pos
              return
     

  At its first call, _update does not update the vehicles orientation and position, but it initializes the calculation. We need the original difference of the wheel positions and we need the wheel positions of the last call. The calculation of the vehicles position is incremental, the calculation of its orientation is absolute.
• The vehicles orientation:

  
                diff = self._pos[1] - self._pos[0] - self._orig_diff
                self._orientation = self._polarity * diff * self._radius_wheel / self._tread
     

  The orientation is calculated from the difference of the two wheel positions. The only additional information, we need, are the dimensions of the vehicle, _radius_wheel, _tread and the original difference, when the first movement started.
• step holds the change of the wheel positions since the last call of _update (incremental).
• There are two ways to determine radius_turn:
  • if _turn is given, we use it and take the following equation (from lesson 4):

    
    turn = 100 * (1 - (radius_turn - 0.5 * tread) / (radius_turn + 0.5 * tread))
           

    From this equation we isolate radius_turn and get:

    
    radius_turn = self._tread * (100 / self._turn - 0.5)
           

  • else we use this equation (from lesson 4):

    
    step_right / step_left = (radius_turn + 0.5 * tread) / (radius_turn - 0.5 * tread)
           

    and isolate radius_turn:

    
    radius_turn = 0.5 * self._tread * (step[1] + step[0]) / (step[1] - step[0])
           

• If the motor was moved between the end of the last movement and the start of the next (overshooting or passive movement f.i. by hand), this movement will update position and orientation. But we calculate the movement in a linear way! If f.i. the rigth wheel was moved 360° and the left 180° we take it as a turn to the left with constant radius_wheel = 1.5 * tread. This is the meaning of:

  
              if not self._moves:
                  radius_turn = 0.5 * self._tread * (step[1] + step[0]) / (step[1] - step[0])
     

  Please keep in mind, that this calculation is a cause of errors. F.i. if you move your vehicle by hand on a nonlinear course, the next calculation of the orientation will be correct, but not the vehicles position. But for overshooting movements, this is a good assumption. The vehicle keeps its actual movement and slows down by the mechanical friction.
• The sign of radius_turn indicates the direction of the turn. Positive stands for turns to the left, negative for turns to the right.
• We hold angle in the range [-180 - 180°]:

  
      angle += 180
      angle %= 360
      angle -= 180
     

• To the outside we keep the orientation in the range [-180 - 180°] but internally we count all circles:

  
      @property
      def orientation(self):
          o_tmp = self._orientation + 180
          o_tmp %= 360
          return o_tmp - 180
     

Changing method move

The changes of method move are straight forward:

    def move(self, speed: int, turn: int) -> None:
        assert self._sync_mode != ev3.SYNC, 'no unlimited operations allowed in sync_mode SYNC'
        assert isinstance(speed, int), "speed needs to be an integer value"
        assert -100 <= speed and speed <= 100, "speed needs to be in range [-100 - 100]"
        assert isinstance(turn, int), "turn needs to be an integer value"
        assert -200 <= turn and turn <= 200, "turn needs to be in range [-200 - 200]"
        if self._polarity == -1:
            speed *= -1
        if self._port_left < self._port_right:
            turn *= -1
        ops_start = b''.join([
            ev3.opOutput_Step_Sync,
            ev3.LCX(0),                                  # LAYER
            ev3.LCX(self._port_left + self._port_right), # NOS
            ev3.LCX(speed),
            ev3.LCX(turn),
            ev3.LCX(0),                                  # STEPS
            ev3.LCX(0),                                  # BRAKE
            ev3.opOutput_Start,
            ev3.LCX(0),                                  # LAYER
            ev3.LCX(self._port_left + self._port_right)  # NOS
        ])
        reply = self.send_direct_cmd(ops_start + self._ops_pos(), global_mem=8)
        pos = struct.unpack('<ii', reply[5:])
        if self._port_left < self._port_right:
            turn *= -1
        self._update(pos)
        self._turn = turn
        self._moves = True
      

Remarks:

• Here I showed you all the tests of the input values. I think it's good style to do so, but you can argue, that it's waste of time. Maybe you are right, my experience says it's the opposite.
• We first call method _update and then set _turn to its new value. Please keep in mind, the first call of method _update does the initialization or finishes the last movement (maybe some overshooting after stopping). Then the first incremental step of the new movement starts.

Changing method stop

We also add some code to method stop:

    def stop(self, brake: bool=False) -> None:
        if brake:
            br = 1
        else:
            br = 0
        ops_stop = b''.join([
            ev3.opOutput_Stop,
            ev3.LCX(0),                                  # LAYER
            ev3.LCX(self._port_left + self._port_right), # NOS
            ev3.LCX(br)                                  # BRAKE
        ])
        reply = self.send_direct_cmd(ops_stop + self._ops_pos(), global_mem=8)
        pos = struct.unpack('<ii', reply[5:])
        self._update(pos)
        self._turn = None
        self._moves = False
      

We need no more changes, the solution to the first of our deficits is found and the coding is finished.

Tests

This allows to add some code to the remote controlled vehicle (lesson 3), that shows its actual position and orientation:

#!/usr/bin/env python3

import curses
import ev3, ev3_vehicle

def react(c):
    global speed, turn, vehicle
    if c in [ord('q'), 27, ord('p')]:
        vehicle.stop()
        return
    elif c == curses.KEY_LEFT:
        turn += 5
        turn = min(turn, 200)
    elif c == curses.KEY_RIGHT:
        turn -= 5
        turn = max(turn, -200)
    elif c == curses.KEY_UP:
        speed += 5
        speed = min(speed, 100)
    elif c == curses.KEY_DOWN:
        speed -= 5
        speed = max(speed, -100)
    vehicle.move(speed, turn)
    stdscr.addstr(5, 0, 'speed: {}, turn: {}      '.format(speed, turn))
    stdscr.addstr(6, 0, 'x: {}, y: {}, o: {}      '.format(vehicle.pos_x, vehicle.pos_y, vehicle.orientation))

def main(window) -> None:
    global stdscr
    stdscr = window
    stdscr.clear()      # print introduction
    stdscr.refresh()
    stdscr.addstr(0, 0, 'Use Arrows to navigate your EV3-vehicle')
    stdscr.addstr(1, 0, 'Pause your vehicle with key <p>')
    stdscr.addstr(2, 0, 'Terminate with key <q>')

    while True:
        c = stdscr.getch()
        if c in [ord('q'), 27]:
            react(c)
            break
        elif c in [ord('p'),
                   curses.KEY_RIGHT, curses.KEY_LEFT, curses.KEY_UP, curses.KEY_DOWN]:
            react(c)

speed = 0
turn  = 0   
vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128, # radius_wheel
    0.1175,  # tread
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
stdscr = None

curses.wrapper(main)
      

Every call of method move actualizes the vehicles position and orientation. I hope, this demonstrates the potential and helps to test the correctness of your code. From now on, our vehicle knows where it is.

Some more tests

I tested my methods with this little program:

#!/usr/bin/env python3

import ev3, ev3_vehicle, time

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,
    0.1175,
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
speed = 25
vehicle.move(speed, 50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.move(-speed, -50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.move(speed, 50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.stop()
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
      

the output:

0.0 0.0 0.0
0.1637335636023255 0.1110167059693961 68.27710638297873
0.21502032005773686 -0.08993607104896707 140.35744680851064
0.01860015951977584 -0.07069575789507432 -151.54655319148935
      

The movement is correct, left turn forward, right turn backward, left turn forward, then stop. Coordinates and orientations fit the movements. Seems to be o.k.

Different motor ports

I changed the motor ports and the program:

#!/usr/bin/env python3

import ev3, ev3_vehicle, time

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,
    0.1175,
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
vehicle.port_left = ev3.PORT_A
vehicle.port_right = ev3.PORT_D
speed = 25
vehicle.move(speed, 50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.move(-speed, -50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.move(speed, 50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.stop()
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
      

the output:

0.0 0.0 0.0
0.1637335636023255 0.1110167059693961 68.27710638297873
0.2133133692712118 -0.08850352729832701 139.63302127659574
0.016659449916087088 -0.06704135461054374 -152.08987234042553
      

O.k., the movements look unchanged, but we see small deviations of coordinates and orientation.

Different polarity

A third test with polarity. I defined, that the back of my vehicle becomes its front. That says i inverted the direction of my vehicle and changed my program:

#!/usr/bin/env python3

import ev3, ev3_vehicle, time

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,
    0.1175,
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
vehicle.polarity = -1
speed = 25
vehicle.move(speed, 50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.move(-speed, -50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.move(speed, 50)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
time.sleep(3)
vehicle.stop()
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
      

the output:

0.0 0.0 0.0
0.16454523888049474 0.11309204830933074 69.00153191489363
0.21880260504386045 -0.08754552962293967 141.26297872340427
0.022102904040755295 -0.0714118151929866 -150.64102127659578
      

O.k., same movements, small deviations of the values.

Improving the depressed giraffe

We come back to a topic of lesson 5, the depressed giraffe. I see two improvements:

• To increase the feeling of futility, we add two final movements and let the vehicle return to its original position and orientation.
• We make the vehicle an explorer and print the coordinates, where it sees barriers and abysses.

The program:

#!/usr/bin/env python3

import ev3, ev3_vehicle, struct, random, math

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128, # radius_wheel
    0.1175,  # tread
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)

def distance(ev3_obj: ev3.EV3) -> float:
    ops = b''.join([
        ev3.opInput_Device,
        ev3.READY_SI,
        ev3.LCX(0),          # LAYER
        ev3.LCX(0),          # NO
        ev3.LCX(33),         # TYPE - EV3-IR
        ev3.LCX(0),          # MODE - Proximity
        ev3.LCX(1),          # VALUES
        ev3.GVX(0)           # VALUE1
    ])
    reply = ev3_obj.send_direct_cmd(ops, global_mem=4)
    return struct.unpack('<f', reply[5:])[0]

def output(vehicle, dist) -> None:
    orientation = vehicle.orientation
    coord_x = vehicle.pos_x + 0.22 * math.cos(math.radians(orientation))
    coord_y = vehicle.pos_y + 0.22 * math.sin(math.radians(orientation))
    if dist < 15:
        print("barrier at position:", coord_x, coord_y)
    else:
        print("abyss   at position:", coord_x, coord_y)

speed = 25
vehicle.move_straight(speed)
for i in range(10):
    while True:
        dist = distance(vehicle)
        if dist < 13 or dist > 20:
            vehicle.stop()
            output(vehicle, dist)
            break
    angle = 135 + 45 * random.random()
    if random.random() > 0.5:
        vehicle.drive_turn(speed, 0, angle)
    else:
        vehicle.drive_turn(speed, 0, angle, right_turn=True)
    speed -= 1    
    vehicle.move_straight(speed)
vehicle.drive_to(speed, 0, 0)
vehicle.rotate_to(speed, 0)
      

Remarks;

• My vehicle looks 22 cm forwards.
• sync_mode is STD and is never changed.

its output:

barrier at position: 0.4432150449594604 0.0
abyss   at position: -0.23650439560294192 -0.4323822005763357
barrier at position: 0.4516783940018264 0.0027074661262277283
abyss   at position: -0.5108628985856991 -0.2366051578496351
barrier at position: 0.4309589540056278 -0.3475941113648004
abyss   at position: -0.24979571049415147 0.4114525146650798
abyss   at position: -0.039353364703376706 -0.46545993360735577
abyss   at position: 0.1684999421049867 0.4478658984910829
abyss   at position: 0.36909815313865585 -0.45385386123569804
abyss   at position: 0.06286411648495513 0.4259295066377581
      

Non blocking motor movements

We already discussed, that our actual version of exact movements (methods drive_straight, drive_turn, rotate_to and drive_to) block the EV3 device and are not compatible with multitasking. Now we will change that. The idea is to interrupt an unlimited movement just in the moment, when it reaches its final position.

A first attempt

We code a helper method:

    def _test_pos(
            self,
            direction: float,
            final_pos: list
    ) -> bool:
        reply = self.send_direct_cmd(self._ops_pos(), global_mem=8)
        pos = struct.unpack('<ii', reply[5:])
        self._update(pos)
        if direction > 0 and self._pos[0] >= final_pos[0] or \
           direction < 0 and self._pos[0] <= final_pos[0]:
            return False
        else:
            return True
      

and we change method drive_straight to:

    def drive_straight(self, speed: int, distance: float=None) -> None:
        self.move(speed, 0)
        if distance != None:
            step = round(distance * 360 / (2 * math.pi * self._radius_wheel))
            direction = math.copysign(1, speed * self._polarity)
            final_pos = [self._pos[0] + direction * step,
                         self._pos[1] + direction * step]
            while self._test_pos(direction, final_pos): pass
      

Some annotations:

• We start an unlimited movement:

  
          self.move(speed, 0)
     

  This updates the attributes _pos.
• We calculate final_pos, the final positions of the motors:

  
              step = round(distance * 360 / (2 * math.pi * self._radius_wheel))
              direction = math.copysign(1, speed * self._polarity)
              final_pos = [self._pos[0] + direction * step,
                           self._pos[1] + direction * step]
     

• In a loop we call method _test_pos until it returns False:

  
              while self._test_pos(direction, final_pos): pass
     

• When the loop is finished, control comes back (we will call method stop).
• This is close to the logic of our depressed giraffe. The value of a sensor regulates the end of a loop.
• The only drawback is the unneeded high data traffic between the computer that runs our program and the EV3 device.

I tested it with this little program:

#!/usr/bin/env python3

import ev3, ev3_vehicle, time

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,
    0.1175,
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
vehicle.verbosity = 1
speed = 25
vehicle.drive_straight(speed, 0.20)
vehicle.stop()
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
      

its output:

21:39:05.402402 Sent 0x|1F:00|2A:00|00|08:00|B0:00:09:19:00:00:00:A6:00:09:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:39:05.481193 Recv 0x|0B:00|2A:00|02|4C:02:00:00:51:02:00:00|
21:39:05.481850 Sent 0x|15:00|2B:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:39:05.516491 Recv 0x|0B:00|2B:00|02|4E:02:00:00:54:02:00:00|
21:39:05.517042 Sent 0x|15:00|2C:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:39:05.546227 Recv 0x|0B:00|2C:00|02|53:02:00:00:59:02:00:00|
...
21:39:07.662067 Sent 0x|15:00|5F:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:39:07.704005 Recv 0x|0B:00|5F:00|02|6F:04:00:00:75:04:00:00|
21:39:07.704671 Sent 0x|19:00|60:00|00|08:00|A3:00:09:00:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:39:07.740012 Recv 0x|0B:00|60:00|02|7A:04:00:00:7F:04:00:00|
0.2072445841720114 0.0 0.0
      

The result is good, we wanted 20 cm, we got 20.72 cm. This approach needed 55 direct commands in 2.34 sec.! This is what bluetooth is able to transport, one cycle per 0.043 sec. (depends of the quality of the connection, f.i. distance). Let's look, what USB does:

21:44:18.199948 Sent 0x|1F:00|2A:00|00|08:00|B0:00:09:19:00:00:00:A6:00:09:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:44:18.209765 Recv 0x|0B:00|2A:00|02|36:02:00:00:43:02:00:00|
21:44:18.210349 Sent 0x|15:00|2B:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:44:18.215694 Recv 0x|0B:00|2B:00|02|36:02:00:00:43:02:00:00|
21:44:18.216260 Sent 0x|15:00|2C:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:44:18.223820 Recv 0x|0B:00|2C:00|02|37:02:00:00:43:02:00:00|
...
21:44:20.365853 Sent 0x|15:00|30:01|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:44:20.373271 Recv 0x|0B:00|30:01|02|51:04:00:00:5F:04:00:00|
21:44:20.373998 Sent 0x|19:00|31:01|00|08:00|A3:00:09:00:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:44:20.383276 Recv 0x|0B:00|31:01|02|53:04:00:00:61:04:00:00|
0.2009306810699958 0.0 0.0
      

Now we have 264 direct commands, USB is much faster than bluetooth. The error is smaller because of the faster reaction. But back to the high traffic. In principle this is no problem, there is no queue of waiting commands. If another task sends a direct command, this will wait for one cycle and not more. But if other tasks also send such series of commands, waiting becomes the standard and the reaction time will increase (0.043 sec. per task with bluetooth, about 0.01 sec. with USB). We search for a solution, that reduces the traffic without loosing precision.

Second attempt with reduced data traffic

Let's work on an improved version. We add some variables to the constructor:

    def __init__(self, radius_wheel: float, tread: float,
                 protocol: str=None,
                 host: str=None,
                 ev3_obj: ev3.EV3=None):
        super().__init__(protocol=protocol,
                         host=host,
                         ev3_obj=ev3_obj)
        self._radius_wheel = radius_wheel
        self._tread = tread
        self._polarity = 1
        self._port_left = ev3.PORT_D
        self._port_right = ev3.PORT_A
        self._orientation = 0.0
        self._pos_x = 0.0
        self._pos_y = 0.0
        self._orig_diff = None
        self._pos = None
        self._turn = None
        self._moves = False
        self._last_t = None
        self._last_o = None
        self._last_pos = None
        self._to_stop = False
      

Their meaning:

• _last_t: time of the last values of the wheel position and the vehicles orientation.
• _last_o: last orientation of the vehicle.
• _last_pos: last position of the wheels.
• _to_stop: Flag if the next call (of _test_pos or _test_o) will be the last one.

The new method _test_pos:

    def _test_pos(
            self,
            direction: float,
            final_pos: list
    ) -> float:
        if self._to_stop:
            self._to_stop = False
            self._last_t = None
            self._update(final_pos)
            return -1
        if not self._last_t:
            first_call = True
            wait = 0.1
        else:
            first_call = False
            reply = self.send_direct_cmd(self._ops_pos(), global_mem=8)
            pos = struct.unpack('<ii', reply[5:])
            self._update(pos)
            if direction > 0 and self._pos[0] >= final_pos[0] or \
               direction < 0 and self._pos[0] <= final_pos[0]:
                self._last_t = None
                return -1
            delta_t = time.time() - self._last_t
            delta_pos = [self._pos[0] - self._last_pos[0],
                         self._pos[1] - self._last_pos[1]]
        self._last_t = time.time()
        self._last_pos = self._pos
        if first_call:
            pass
        elif abs(delta_pos[0]) < 0.001:
            wait = 2*delta_t
        else:
            rest_pos = final_pos[0] - self._pos[0]
            rest_t = delta_t * rest_pos / delta_pos[0] - self._reaction()
            delta_t_new = min(2, 2*delta_t)
            if rest_t < (delta_t_new + 0.1):
                self._to_stop = True
                wait = rest_t
            else:
                wait = delta_t_new
        return wait
    

Some remarks:

• The signature has changed in the type of the returned value. Now it's a number, which is the time gap (in seconds) to the next call of _test_pos. The value -1 signals the caller, that it has to finish its loop.
• We need three variables with values from the last call:
  • _last_t holds the time, when the last execution took place. If it holds value None, this indicates the first execution in the loop.
  • _last_pos holds the motor positions from the last execution.
  • _to_stop is a flag. When set, no sensor data will be red, but the caller gets the signal to finish the loop.
• If the motor accidently overshot its final position, the loop has to be finished too:

  
              if direction > 0 and self._pos[0] >= final_pos[0] or \
                 direction < 0 and self._pos[0] <= final_pos[0]:
                  self._last_t = None
                  return -1
     

• wait is the waiting time to the next execution. delta_t is the time gap since the last execution. We start with 0.1 sec. and double it per call (with a maximum of 2 sec.).
• We calculate the expected rest time rest_t until the movement reaches its final position. This value is in seconds.
• We subtract some reaction time to get a better precision. It depends from the type of connection.
• When _test_pos is called its last time (_to_stop == True), it does not ask for the actual wheel positions. Instead it calls _update with final_pos. If there is a deviation, method _update will correct it at its next call. This seems a good algorithm! A final asking of the wheel positions needs time and results in a worse precision. The next command (f.i. a call of method stop) would be late.

The new method drive_straight

We change drive_straight once more:

    def drive_straight(self, speed: int, distance: float=None) -> None:
        self.move_straight(speed)
        if distance != None:
            step = round(distance * 360 / (2 * math.pi * self._radius_wheel))
            direction = math.copysign(1, speed * self._polarity)
            final_pos = [self._pos[0] + direction * step,
                         self._pos[1] + direction * step]
            while True:
                value = self._test_pos(direction, final_pos)
                if value == -1:
                    break
                time.sleep(value)
      

Test of the new methods

We use the same program as above to test it. Its output is much shorter now:

21:54:51.153759 Sent 0x|1F:00|2A:00|00|08:00|B0:00:09:19:00:00:00:A6:00:09:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:54:51.241695 Recv 0x|0B:00|2A:00|02|8B:0D:00:00:81:0D:00:00|
21:54:51.343731 Sent 0x|15:00|2B:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:54:51.380502 Recv 0x|0B:00|2B:00|02|A0:0D:00:00:96:0D:00:00|
21:54:51.622511 Sent 0x|15:00|2C:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:54:51.659577 Recv 0x|0B:00|2C:00|02|EC:0D:00:00:E1:0D:00:00|
21:54:52.179277 Sent 0x|15:00|2D:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:54:52.215474 Recv 0x|0B:00|2D:00|02|7A:0E:00:00:70:0E:00:00|
21:54:53.350092 Sent 0x|19:00|2E:00|00|08:00|A3:00:09:00:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
21:54:53.392616 Recv 0x|0B:00|2E:00|02|A3:0F:00:00:98:0F:00:00|
0.19907351344522178 -8.687425881730374e-05 -0.18110638297872583
      

This reduces the data traffic to 5 direct commands and has a better precision! We can adjust the method _reaction(), which also includes the reaction of the motors. In case of Bluetooth 0.043 sec. seems to be a good value. If you look at the times, you can see how the gap between two commands grows.

The new method drive_turn

drive_turn is similar to drive_straight:

    def drive_turn(
            self,
            speed: int,
            radius_turn: float,
            angle: float=None,
            right_turn: bool=False
    ) -> None:
        self.move_turn(speed, radius_turn, right_turn=right_turn)
        if angle != None:
            step_outer = self._polarity * angle * (radius_turn + 0.5 * self._tread) / self._radius_wheel
            step_inner = self._polarity * angle * (radius_turn - 0.5 * self._tread) / self._radius_wheel
            if radius_turn >= 0 and not right_turn:
                direction = math.copysign(1, speed)
                final_pos = [self._pos[0] + direction * step_inner,
                             self._pos[1] + direction * step_outer]
            else:
                direction = - math.copysign(1, speed)
                final_pos = [self._pos[0] - direction * step_outer,
                             self._pos[1] - direction * step_inner]
            final_o = self._orientation + direction * angle
            while True:
                value = self._test_o(direction, final_o, final_pos)
                if value == -1:
                    break
                time.sleep(value)
      

We call _test_o instead of _test_pos:

    def _test_o(
            self,
            direction: float,
            final_o: float,
            final_pos: list
    ) -> float:
        if self._to_stop:
            self._to_stop = False
            self._last_t = None
            self._update(final_pos)
            return -1
        if not self._last_t:
            first_call = True
            wait = 0.1
        else:
            first_call = False
            reply = self.send_direct_cmd(self._ops_pos(), global_mem=8)
            pos = struct.unpack('<ii', reply[5:])
            self._update(pos)
            if direction > 0 and self._orientation >= final_o or \
               direction < 0 and self._orientation <= final_o:
                self._last_t = None
                return -1
            delta_t = time.time() - self._last_t
            delta_o = self._orientation - self._last_o
            delta_pos = [self._pos[0] - self._last_pos[0],
                         self._pos[1] - self._last_pos[1]]
        self._last_t = time.time()
        self._last_o = self._orientation
        self._last_pos = self._pos
        if first_call:
            if abs(final_o - self._orientation) < 1:
                self._last_t = None
                return -1
            else:
                pass
        elif abs(delta_o) < 0.5:
            wait = 2*delta_t
        else:
            rest_o = final_o - self._orientation
            rest_t = delta_t * rest_o / delta_o - self._reaction()
            delta_t_new = min(2, 2*delta_t)
            if rest_t < (delta_t_new + 0.1):
                self._to_stop = True
                wait = rest_t
            else:
                wait = delta_t_new
        return wait
      

Test of method drive_turn

The following program calls method drive_turn three times. It drives three turns of 60°, which build a course, that ends at its start. A final rotate_to will rotate it back to its initial orientation:

#!/usr/bin/env python3

import ev3, ev3_vehicle, time

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,                 # radius_wheel
    0.1175,                  # tread
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
vehicle.verbosity = 1
speed = 25
vehicle.drive_turn(speed, 0.25, 60)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
vehicle.drive_turn(-speed, -0.25, 60)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
vehicle.drive_turn(speed, 0.25, 60)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
vehicle.rotate_to(speed, 0)
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
vehicle.stop()
print(vehicle.pos_x, vehicle.pos_y, vehicle.orientation)
      

This programs output:

08:07:34.626601 Sent 0x|20:00|2A:00|00|08:00|B0:00:09:19:81:26:00:00:A6:00:09:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:34.671391 Recv 0x|0B:00|2A:00|02|00:00:00:00:00:00:00:00|
08:07:34.772405 Sent 0x|15:00|2B:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:34.809324 Recv 0x|0B:00|2B:00|02|0F:00:00:00:1A:00:00:00|
08:07:35.086688 Sent 0x|15:00|2C:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:35.123281 Recv 0x|0B:00|2C:00|02|40:00:00:00:6B:00:00:00|
08:07:35.752524 Sent 0x|15:00|2D:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:35.788293 Recv 0x|0B:00|2D:00|02|A9:00:00:00:0F:01:00:00|
08:07:37.121123 Sent 0x|15:00|2E:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:37.165433 Recv 0x|0B:00|2E:00|02|7D:01:00:00:67:02:00:00|
0.2169051784346946 0.12523026315789473 60.0
08:07:38.140368 Sent 0x|20:00|2F:00|00|08:00|B0:00:09:27:81:DA:00:00:A6:00:09:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:38.172256 Recv 0x|0B:00|2F:00|02|1B:02:00:00:65:03:00:00|
08:07:38.273279 Sent 0x|15:00|30:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:38.309238 Recv 0x|0B:00|30:00|02|0B:02:00:00:61:03:00:00|
08:07:38.584359 Sent 0x|15:00|31:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:38.620285 Recv 0x|0B:00|31:00|02|BA:01:00:00:2B:03:00:00|
08:07:39.243873 Sent 0x|15:00|32:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:39.281220 Recv 0x|0B:00|32:00|02|17:01:00:00:C4:02:00:00|
08:07:40.605239 Sent 0x|15:00|33:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:40.642235 Recv 0x|0B:00|33:00|02|C4:FF:FF:FF:F2:01:00:00|
0.21536315282461824 -0.12611634250727966 119.76510638297873
08:07:41.691379 Sent 0x|20:00|34:00|00|08:00|B0:00:09:19:81:26:00:00:A6:00:09:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:41.728148 Recv 0x|0B:00|34:00|02|B0:FE:FF:FF:47:01:00:00|
08:07:41.829186 Sent 0x|15:00|35:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:41.866945 Recv 0x|0B:00|35:00|02|B8:FE:FF:FF:5A:01:00:00|
08:07:42.146330 Sent 0x|15:00|36:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:42.183203 Recv 0x|0B:00|36:00|02|EF:FE:FF:FF:AC:01:00:00|
08:07:42.816792 Sent 0x|15:00|37:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:42.853144 Recv 0x|0B:00|37:00|02|58:FF:FF:FF:56:02:00:00|
08:07:44.195083 Sent 0x|15:00|38:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:44.240279 Recv 0x|0B:00|38:00|02|2D:00:00:00:AD:03:00:00|
-0.0010300902058044126 -0.0023331090393255167 -179.92646808510636
08:07:45.248467 Sent 0x|21:00|39:00|00|08:00|B0:00:09:19:82:C8:00:00:00:A6:00:09:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:45.295099 Recv 0x|0B:00|39:00|02|D5:00:00:00:BB:04:00:00|
08:07:45.396213 Sent 0x|15:00|3A:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:45.434263 Recv 0x|0B:00|3A:00|02|C4:00:00:00:DB:04:00:00|
08:07:45.713570 Sent 0x|15:00|3B:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:45.749108 Recv 0x|0B:00|3B:00|02|6C:00:00:00:29:05:00:00|
08:07:46.380092 Sent 0x|15:00|3C:00|00|08:00|99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:46.426228 Recv 0x|0B:00|3C:00|02|BE:FF:FF:FF:D2:05:00:00|
-0.003961600820337176 -0.002354028059665772 0.6706382978723013
08:07:47.245009 Sent 0x|19:00|3D:00|00|08:00|A3:00:09:00:99:1C:00:13:07:01:01:60:99:1C:00:10:07:00:01:64|
08:07:47.281024 Recv 0x|0B:00|3D:00|02|E6:FE:FF:FF:A9:06:00:00|
-0.003961600820337176 -0.002354028059665772 -0.14161702127648823
      

This looks great! Maybe this was good luck, an error of a few mm or a part of one degree. Let's compare with the optimum accuracy. Our sensor has an accuracy of one degree. This says we get an error of position of:

   delta_x = 2 * pi * radius_wheel / 360
      

In my case, this are 0.00037 m (0.37 mm). We have three movements, the real accuracy is a factor of 4 worse than the theoretical optimum. That sounds good. Let's look at the accuracy of orientation:

   delta_o = 1° * radius_wheel / tread
      

In my case: 0.18°. Our real errors are not worse than the theoretical ones. Here the errors of the four movements must not be added. Our calculation is absolute and not incremental, the calculation of the orientation does not dependent from the movements of the past.

Another source of errors are the exactness of the vehicles dimensions tread and radius_wheel and the fact, that the wheels may spin. The algorithm assumes a steady speed. If it varies, this also will produce errors.

Conclusion

We improved class TwoWheelVehicle. Its API is unchanged, but under the surface, we have done substantial changes. In my eyes, it is a result of good design to keep the face to the outside stable, even when the internals change. The only thing, that was changed (from the ouside point of view), now it needs a final call of method stop to finish a movement.

The title of this lesson was The perfect Subclass. I do not believe, that class TwoWheelVehicle is a perfect subclass. We see overshooting and very hard changes of the movement. We loose accuracy when we work with high velocities or small movements. Nevertheless my résumé is positive, class TwoWheelVehicle does its job very well.

Let's look at the qualities of a perfect subclass of class EV3:

• It has a clear focus and provides a good API to the outside, that hides technical details from its users.
• It uses well defined resources of the EV3 device (in case of class TwoWheelVehicle these are two large motors which drive the left and the right wheel).
• It's independent, that says it does not influence foreign tasks and is not influenced by foreign tasks.
• There is no blocking of the EV3 device.
• It's methods are synchrone, that says control comes back when a task is finished.
• It causes a low data traffic.

All this together allows the parallel execution of multiple tasks (I use task in its custom meaning. I do not understand multitasking as using multiple processes). You may ask, how to manage that, when we loose control until a task ends. The answer is multithreading. If we have multiple parallel tasks, we will have multiple threads. One task, that runs in the base thread (or is joined) makes the timing.

That says, we will omit operations like opSound_Ready (lesson 2) or opOutput_Ready (lesson 4). We will use interruption instead. All the timing must be done by our program and not by the EV3 device.

From this point of view, TwoWheelVehicle is a perfect class. What we need are tools to manage multiple tasks. This will be the topic of the next lessons.