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.

Lesson 5 - Reading data from EV3's sensors

Reading type and mode of a sensor

We start with some self-reflection of the EV3 device and ask it:

1. Wich kind of device is connected to port 16?
2. What's the mode of the sensort at port 16?

Please send the following direct command to your EV3:

----------------------------------------               
 \ len \ cnt \ty\ hd  \op\cd\la\no\ty\mo\              
  ----------------------------------------             
0x|0B:00|2A:00|00|02:00|99|05|00|10|60|61|             
  ----------------------------------------             
   \ 11  \ 42  \re\ 0,2 \I \G \0 \16\0 \1 \             
    \     \     \  \     \n \E \  \  \  \  \            
     \     \     \  \     \p \T \  \  \  \  \           
      \     \     \  \     \u \_ \  \  \  \  \          
       \     \     \  \     \t \T \  \  \  \  \         
        \     \     \  \     \_ \Y \  \  \  \  \        
         \     \     \  \     \D \P \  \  \  \  \       
          \     \     \  \     \e \E \  \  \  \  \      
           \     \     \  \     \v \M \  \  \  \  \     
            \     \     \  \     \i \O \  \  \  \  \    
             \     \     \  \     \c \D \  \  \  \  \   
              \     \     \  \     \e \E \  \  \  \  \  
               ----------------------------------------
      

A few remarks:

• We used operation opInput_device with CMD GET_TYPEMODE
• If used as sensor port, motor port A has the number 16.
• We will get two numbers as answer, type and mode.
• Type needs one byte and will be placed at byte 0, mode also needs one byte and will be placed at byte 1.

I got the following answer:

----------------------    
 \ len \ cnt \rs\ty\mo\   
  ----------------------  
0x|05:00|2A:00|02|07|00|  
  ----------------------  
   \ 5   \ 42  \ok\7 \0 \  
    ----------------------
      

which says: the sensor at motor port A is of type 7 and actually in mode 0. If you take a look at document EV3 Firmware Developer Kit, chapter 5, which has the title Device type list, you find, that type 7 and mode 0 stand for EV3-Large-Motor-Degree.

Reading the actual position of a motor

We come to the really interesting question: What's the actual position of the motor at motor port A? I sent this command:

----------------------------------------------                  
 \ len \ cnt \ty\ hd  \op\cd\la\no\ty\mo\va\v1\                 
  ----------------------------------------------                
0x|0D:00|2A:00|00|04:00|99|1C|00|10|07|00|01|60|                
  ----------------------------------------------                
   \ 13  \ 42  \re\ 0,4 \I \R \0 \16\E \D \1 \0 \                
    \     \     \  \     \n \E \  \  \V \e \  \  \               
     \     \     \  \     \p \A \  \  \3 \g \  \  \              
      \     \     \  \     \u \D \  \  \- \r \  \  \             
       \     \     \  \     \t \Y \  \  \L \e \  \  \            
        \     \     \  \     \_ \_ \  \  \a \e \  \  \           
         \     \     \  \     \D \R \  \  \r \  \  \  \          
          \     \     \  \     \e \A \  \  \g \  \  \  \         
           \     \     \  \     \v \W \  \  \e \  \  \  \        
            \     \     \  \     \i \  \  \  \- \  \  \  \       
             \     \     \  \     \c \  \  \  \M \  \  \  \      
              \     \     \  \     \e \  \  \  \o \  \  \  \     
               \     \     \  \     \  \  \  \  \t \  \  \  \    
                \     \     \  \     \  \  \  \  \o \  \  \  \   
                 \     \     \  \     \  \  \  \  \r \  \  \  \  
                  ----------------------------------------------

      

and I got the reply:

----------------------------    
 \ len \ cnt \rs\ degrees   \   
  ----------------------------  
0x|07:00|2A:00|02|00:00:00:00|  
  ----------------------------  
   \ 7   \ 42  \ok\ 0         \  
    ----------------------------
      

Then I moved the motor by hand and again sent the identical direct command. This time, the reply was:

----------------------------    
 \ len \ cnt \rs\ degrees   \   
  ----------------------------  
0x|07:00|2A:00|02|50:07:00:00|  
  ----------------------------  
   \ 7   \ 42  \ok\ 1872      \  
    ----------------------------

      

which says: the movement of the motor was 1.872 degrees (5.2 rotations). This seems to be correct!

The technical details

It's time to take a look behind the curtain! You need to understand:

• the systematics of the port numbers,
• the parameters of the operations, we used and
• how to address and unpack the global memory.

The systematics of the port numbers

There are four ports for sensors and four ports for motors. The systematics of the sensor port numbers 1 to 4 is:

• port 1: PORT = 0x|00| or LCX(0)
• port 2: PORT = 0x|01| or LCX(1)
• port 3: PORT = 0x|02| or LCX(2)
• port 4: PORT = 0x|03| or LCX(3)

This seems a bit funny, but computers are used to start counting from number 0, humans from number 1. We just learned, that motors are sensors too, from where we can read the actual position of the motors. The motor ports are labeled with the letters A to D, but addressed as:

• port A: PORT = 0x|10| or LCX(16)
• port B: PORT = 0x|11| or LCX(17)
• port C: PORT = 0x|12| or LCX(18)
• port D: PORT = 0x|13| or LCX(19)

I added a little function to my module ev3.py:

def port_motor_input(port_output: int) -> bytes:
    """
    get corresponding input motor port (from output motor port)
    """
    if port_output == PORT_A:
        return LCX(16)
    elif port_output == PORT_B:
        return LCX(17)
    elif port_output == PORT_C:
        return LCX(18)
    elif port_output == PORT_D:
        return LCX(19)
    else:
        raise ValueError("port_output needs to be one of the port numbers [1, 2, 4, 8]")
      

that transforms from the motors output ports to their input ports.

Operation opInput_Device

A short description of the two variants of opInput_Device, we have used:

• opInput_Device = 0x|99| with CMD GET_TYPEMODE = 0x|05|:
  Arguments
  • (Data8) LAYER: chain layer number
  • (Data8) NO: port number
  
  Returns
  • (Data8) TYPE: device type
  • (Data8) MODE: device mode
• opInput_Device = 0x|99| with CMD READY_RAW = 0x|1C|:
  Arguments
  • (Data8) LAYER: chain layer number
  • (Data8) NO: port number
  • (Data8) TYPE: device type
  • (Data8) MODE: device mode
  • (Data8) VALUES: number of return values
  
  Returns
  • (Data32) VALUE1: first value received from sensor in the specified mode

Here Data32 says this is a 32 bit signed integer number. Returned data are values, but keep in mind that the return parameters like VALUE1 are called by reference. The reference is an address of the local or global memory. Read the next part for details.

Addressing the global memory

In lesson 2 we introduced constant parameters and local variables. You will remember, we have seen LCS, LC0, LC1, LC2, LC4, LV0, LV1, LV2 and LV4 and we wrote three functions LCX(value:int), LVX(value:int) and LCS(value:str):

FUNCTIONS
    LCS(value:str) -> bytes
        pack a string into a LCS
    
    LCX(value:int) -> bytes
        create a LC0, LC1, LC2, LC4, dependent from the value
    
    LVX(value:int) -> bytes
        create a LV0, LV1, LV2, LV4, dependent from the value
      

We talked about the identification byte, which defines the type and length of the variables.

Now we code another function GVX, which returns addresses of the global memory. As you already know, bit 0 of the identification byte stands for short or long format:

• 0b 0... .... short format (only one byte, the identification byte includes the value),
• 0b 1... .... long format (the identification byte does not contain any bit of the value).

If bits 1 and 2 are 0b .11. ...., they stand for global variables, which are addresses of the global memory.

Bits 6 and 7 stand for the length of the following value:

• 0b .... ..00 means variable length,
• 0b .... ..01 means one byte follows,
• 0b .... ..10 says two bytes follow,
• 0b .... ..11 says four bytes follow.

Now we write the 4 global variables as binary masks, we don't need signs because addresses are always positive numbers. V stands for one bit of the address (value).

• GV0: 0b 011V VVVV, 5-bit address, range: 0 - 31, length: 1 byte, identified by leading bits 011.
• GV1: 0b 1110 0001 VVVV VVVV, 8-bit address, range: 0 - 255, length: 2 byte, identified by leading byte 0x|E1|.
• GV2: 0b 1110 0010 VVVV VVVV VVVV VVVV, 16-bit address, range: 0 – 65.536, length: 3 byte, identified by leading byte 0x|E2|.
• GV4: 0b 1110 0011 VVVV VVVV VVVV VVVV VVVV VVVV VVVV VVVV, 32-bit address, range: 0 – 4,294,967,296, length: 5 byte, identified by leading byte 0x|E3|.

A few remarks:

• In direct commands, there is no need for GV4! You remember that the global memory has a maximum of 1019 bytes (1024 - 5).
• Addresses of the global memory must be placed correctly. If you write a 4-byte value into the global memory, its address needs to be 0, 4, 8, ... (a multiple of 4). The same with 2-byte values, their address must be multiples of 2.
• You need to split the global memory into segments of the needed lengths, then use the addresses of the first byte of every segment. In our first example we needed two segements (type and mode) of one byte each. So we use GV0(0) and GV0(1) as addresses.
• The header bytes contain the total length of the global memory (for details read lesson 1). In our examples these are 2 bytes resp. 4 bytes. Don't forget to send correct header bytes!
• Don't work with gaps between the segments! The standard tools like struct.unpack don't like them. Place the 4-byte types in front followed by the 2-byte types and so on. This allows a comfortable coding of the unpacking.

A new module function: GVX

Please add a function GVX(value) to your module ev3, that returns the shortest of the types GV0, GV1, GV2 or GV4, dependent from the value. I have done it, now the documentation of my module ev3 says:

FUNCTIONS
    GVX(value:int) -> bytes
        create a GV0, GV1, GV2, GV4, dependent from the value
    
    LCS(value:str) -> bytes
        pack a string into a LCS
    
    LCX(value:int) -> bytes
        create a LC0, LC1, LC2, LC4, dependent from the value
    
    LVX(value:int) -> bytes
        create a LV0, LV1, LV2, LV4, dependent from the value
    
    port_motor_input(port_output:int) -> bytes
        get corresponding input motor port (from output motor port)
      

Unpacking the global memory

I have mentioned, that there exist good tools for unpacking the global memory. In python3, this tool is struct — Interpret bytes as packed binary data.

One byte unsigned integers

My program for reading mode and type from motor port A:

#!/usr/bin/env python3

import ev3, struct

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

ops = b''.join([
    ev3.opInput_Device,
    ev3.GET_TYPEMODE,
    ev3.LCX(0),                   # LAYER
    ev3.port_motor_input(PORT_A), # NO
    ev3.GVX(0),                   # TYPE
    ev3.GVX(1)                    # MODE
])
reply = my_ev3.send_direct_cmd(ops, global_mem=2)
(type, mode) = struct.unpack('BB', reply[5:])
print("type: {}, mode: {}".format(type, mode))
      

Format 'BB' splits the global memory into two 1-byte unsigned integer values. This programs output:

08:08:13.477998 Sent 0x|0B:00|2A:00|00|02:00|99:05:00:10:60:61|
08:08:13.558793 Recv 0x|05:00|2A:00|02|07:00|
type: 7, mode: 0
      

Four bytes floating point and four bytes signed integer

My program for reading the motor positions of the motors at ports A and D:

#!/usr/bin/env python3

import ev3, struct

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

ops = b''.join([
    ev3.opInput_Device,
    ev3.READY_SI,
    ev3.LCX(0),                   # LAYER
    ev3.port_motor_input(PORT_A), # NO
    ev3.LCX(7),                   # TYPE
    ev3.LCX(0),                   # MODE
    ev3.LCX(1),                   # VALUES
    ev3.GVX(0),                   # VALUE1
    ev3.opInput_Device,
    ev3.READY_RAW,
    ev3.LCX(0),                   # LAYER
    ev3.port_motor_input(PORT_D), # NO
    ev3.LCX(7),                   # TYPE
    ev3.LCX(0),                   # MODE
    ev3.LCX(1),                   # VALUES
    ev3.GVX(4)                    # VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=8)
(pos_a, pos_d) = struct.unpack('<fi', reply[5:])
print("positions in degrees (ports A and D): {} and {}".format(pos_a, pos_d))
      

Format '<fi' splits the global memory into a 4-bytes float and a 4-bytes signed integer, both in little endian. The output:

08:32:32.865522 Sent 0x|15:00|2A:00|00|08:00|99:1D:00:10:07:00:01:60:99:1C:00:13:07:00:01:64|
08:32:32.949266 Recv 0x|0B:00|2A:00|02|00:80:6C:C4:54:04:00:00|
positions in degrees (ports A and D): -946.0 and 1108
      

Strings

We read the brickname of the EV3 device:

#!/usr/bin/env python3

import ev3, struct

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

ops = b''.join([
    ev3.opCom_Get,
    ev3.GET_BRICKNAME,
    ev3.LCX(16),                  # LENGTH
    ev3.GVX(0)                    # NAME
])
reply = my_ev3.send_direct_cmd(ops, global_mem=16)
(brickname,) = struct.unpack('16s', reply[5:])
brickname = brickname.split(b'\x00')[0]
brickname = brickname.decode("ascii")
print("Brickname:", brickname)
      

Remarks:

• Format '16s' describes a string of 16 bytes length.
• brickname = brickname.split(b'\x00')[0] takes the first zero-terminated part of the string. You need to do that because the EV3 device does not clear the global memory. There may be some garbage in the strings right-side part. Wait a moment, then I will demonstrate the problem.
• brickname = brickname.decode("ascii") makes a string type from a bytes type.

This programs output:

08:55:00.098825 Sent 0x|0A:00|2B:00|00|10:00|D3:0D:81:20:60|
08:55:00.138258 Recv 0x|13:00|2B:00|02|6D:79:45:56:33:00:00:00:00:00:00:00:00:00:00:00|
Brickname: myEV3
      

Strings with garbage

We send two direct commands, the second reads a string:

#!/usr/bin/env python3

import ev3, struct

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

ops = b''.join([
    ev3.opInput_Device,
    ev3.READY_SI,
    ev3.LCX(0),                   # LAYER
    ev3.port_motor_input(PORT_A), # NO
    ev3.LCX(7),                   # TYPE
    ev3.LCX(0),                   # MODE
    ev3.LCX(1),                   # VALUES
    ev3.GVX(0),                   # VALUE1
    ev3.opInput_Device,
    ev3.READY_RAW,
    ev3.LCX(0),                   # LAYER
    ev3.port_motor_input(PORT_D), # NO
    ev3.LCX(7),                   # TYPE
    ev3.LCX(0),                   # MODE
    ev3.LCX(1),                   # VALUES
    ev3.GVX(4)                    # VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=8)
(pos_a, pos_d) = struct.unpack('<fi', reply[5:])
print("positions in degrees (ports A and D): {} and {}".format(pos_a, pos_d))

ops = b''.join([
    ev3.opCom_Get,
    ev3.GET_BRICKNAME,
    ev3.LCX(16),                  # LENGTH
    ev3.GVX(0)                    # NAME
])
reply = my_ev3.send_direct_cmd(ops, global_mem=16)
          

This programs output:

09:13:30.379771 Sent 0x|15:00|2A:00|00|08:00|99:1D:00:10:07:00:01:60:99:1C:00:13:07:00:01:64|
09:13:30.433495 Recv 0x|0B:00|2A:00|02|00:08:90:C5:FE:F0:FF:FF|
positions in degrees (ports A and D): -4609.0 and -3842
09:13:30.433932 Sent 0x|0A:00|2B:00|00|10:00|D3:0D:81:20:60|
09:13:30.502499 Recv 0x|13:00|2B:00|02|6D:79:45:56:33:00:FF:FF:00:00:00:00:00:00:00:00|
      

The zero-terminated string 'myEV3' (0x|6D:79:45:56:33:00|) has a length of 6 bytes. The next two bytes (0x|FF:FF|) are garbage from the first direct command.

The fastest thumb

The touch sensor has type number 16 and two modes, 0: EV3-Touch and 1: EV3-Bump. The first tests, if the sensor actually is touched, the second counts the touches since the last clearing of the sensor. We demonstrate these modes with a little program. It counts, how often the touch sensor is bumped within five seconds (please plug your touch sensor into port 2):

#!/usr/bin/env python3

import ev3, struct, time

my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')

def change_color(color) -> bytes:
    return b''.join([
        ev3.opUI_Write,
        ev3.LED,
        color
    ])

def play_sound(vol: int, freq: int, dur:int) -> bytes:
    return b''.join([
        ev3.opSound,
        ev3.TONE,
        ev3.LCX(vol),
        ev3.LCX(freq),
        ev3.LCX(dur)
    ])

def ready() -> None:
    ops = change_color(ev3.LED_RED)
    my_ev3.send_direct_cmd(ops)
    time.sleep(3)

def steady() -> None:
    ops_color = change_color(ev3.LED_ORANGE)
    ops_sound = play_sound(1, 200, 60)
    my_ev3.send_direct_cmd(ops_color + ops_sound)
    time.sleep(0.25)
    for i in range(3):
        my_ev3.send_direct_cmd(ops_sound)
        time.sleep(0.25)

def go() -> None:
    ops_clear = b''.join([
        ev3.opInput_Device,
        ev3.CLR_CHANGES,
        ev3.LCX(0),          # LAYER
        ev3.LCX(1)           # NO
    ])
    ops_color = change_color(ev3.LED_GREEN_FLASH)
    ops_sound = play_sound(10, 200, 100)
    my_ev3.send_direct_cmd(ops_clear + ops_color + ops_sound)
    time.sleep(5)

def stop() -> None:
    ops_read = b''.join([
        ev3.opInput_Device,
        ev3.READY_SI,
        ev3.LCX(0),          # LAYER
        ev3.LCX(1),          # NO
        ev3.LCX(16),         # TYPE - EV3-Touch
        ev3.LCX(0),          # MODE - Touch
        ev3.LCX(1),          # VALUES
        ev3.GVX(0),          # VALUE1
        ev3.opInput_Device,
        ev3.READY_SI,
        ev3.LCX(0),          # LAYER
        ev3.LCX(1),          # NO
        ev3.LCX(16),         # TYPE - EV3-Touch
        ev3.LCX(1),          # MODE - Bump
        ev3.LCX(1),          # VALUES
        ev3.GVX(4)           # VALUE1
    ])
    ops_sound = play_sound(10, 200, 100)
    reply = my_ev3.send_direct_cmd(ops_sound + ops_read, global_mem=8)
    (touched, bumps) = struct.unpack('<ff', reply[5:])
    if touched == 1:
        bumps += 0.5
    print(bumps, "bumps")

for i in range(3):
    ready()
    steady()
    go()
    stop()
ops_color = change_color(ev3.LED_GREEN)
my_ev3.send_direct_cmd(ops_color)
print("**** Game over ****")
      

We used a new operation: opInput_Device = 0x|99| with CMD CLR_CHANGES = 0x|1A| and these arguments:

• (Data8) LAYER: chain layer number
• (Data8) NO: port number

It clears the sensor, all its internal data are set to initial values.

The depressed giraffe

Let's code a program, that uses class TwoWheelVehicle and the infrared sensor. The infrared sensor has type number 33 and its mode 0 reads the free distance in front of the sensor. We use it to detect barriers or abysses in front of a vehicle. Convert your vehicle and place the infrared sensor so that it looks to the front, but from top to down (about 30 - 60 ° downwards). The sensor reads the area in front of the vehicle and stops the movement, when something unexpected happens:

#!/usr/bin/env python3

import ev3, ev3_vehicle, struct, random

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

def distance() -> 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 = vehicle.send_direct_cmd(ops, global_mem=4)
    return struct.unpack('<f', reply[5:])[0]

speed = 25
vehicle.move(speed, 0)
for i in range(10):
    while True:
        dist = distance()
        if dist < 15 or dist > 20:
            break
    vehicle.stop()
    vehicle.sync_mode = ev3.SYNC
    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)
    vehicle.sync_mode = ev3.STD
    speed -= 2
    vehicle.move(speed, 0)
vehicle.stop()
      

Some annotations:

• If you downloaded module ev3_vehicle.py from ev3-python3, please eliminate the settings of property sync_mode (vehicle.sync_mode = ev3.SYNC resp. vehicle.sync_mode = ev3.STD).
• The central part of the algorithm is:

  
      while True:
          dist = distance()
          if dist < 15 or dist > 20:
              break
      vehicle.stop()
     

  this code stops the movement, when the free distance is smaller than 15 cm or larger than 20 cm (values depend from the construction). This says: if the vehicle reaches the edge of a desk (distance becomes large), it will stop and if it reaches a barrier (small distance), it stops too.
• After stopping, the vehicle turns on place in random direction and with a random angle (in the range of 135 to 180°). sync_mode is set to SYNC, we want the program to wait until the turn is done:

  
      vehicle.sync_mode = ev3.SYNC
      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)
      vehicle.sync_mode = ev3.STD
     

• Then the speed decreases and the vehicle moves forward, the cycle starts again.

  
      speed -= 2
      vehicle.move(speed, 0)
     

• The number of the cycles is limited to 10.
• My sensor was placed on a construction that assembles the neck of a giraffe. This and the slower and slower movement made the name.
• One drawback is the sensors focus to the direction straight forwards. If the vehicle moves in a small angle against the edge of the desk or against a barrier, it will recognize it too late.

What happens technically?

• vehicle.move(speed, 0) starts an unlimited movement, which does not block the EV3 device.
• This allows to read the free distance from the sensor while the vehicle moves.
• The similarity to the remote control of lessons 3 and 4 is significiant, the sensor replaces the human mind.
• The only action that blocks the EV3 device ist method drive_turn. This command needs sync_mode = SYNC. Happily we don't need any sensor data, while it's executed.

Now it's at you to adapt this program to your needs and your construction of the vehicle. I found it most impressive to place the vehicle onto a desc, where a part of the table top was separated by a barrier.

Seeker

The infrared sensor has another interesting mode: seeker. This mode reads direction and distance of the EV3 infrared beacon. The beacon allows to select one of four signal channels. Please plug in the IR sensor to port 2, switch on the beacon, select a channel, place it in front of the infrared sensor, then run this program:

#!/usr/bin/env python3

import ev3, struct

my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')

ops_read = b''.join([
    ev3.opInput_Device,
    ev3.READY_RAW,
    ev3.LCX(0),                       # LAYER
    ev3.LCX(1),                       # NO
    ev3.LCX(33),                      # TYPE - IR
    ev3.LCX(1),                       # MODE - Seeker
    ev3.LCX(8),                       # VALUES
    ev3.GVX(0),                       # VALUE1 - heading   channel 1
    ev3.GVX(4),                       # VALUE2 - proximity channel 1
    ev3.GVX(8),                       # VALUE3 - heading   channel 2
    ev3.GVX(12),                      # VALUE4 - proximity channel 2
    ev3.GVX(16),                      # VALUE5 - heading   channel 3
    ev3.GVX(20),                      # VALUE6 - proximity channel 3
    ev3.GVX(24),                      # VALUE5 - heading   channel 4
    ev3.GVX(28)                       # VALUE6 - proximity channel 4
])
reply = my_ev3.send_direct_cmd(ops_read, global_mem=32)
(
    h1, p1,
    h2, p2,
    h3, p3,
    h4, p4,
) = struct.unpack('8i', reply[5:])
print("heading1: {}, proximity1: {}".format(h1, p1))
print("heading2: {}, proximity2: {}".format(h2, p2))
print("heading3: {}, proximity3: {}".format(h3, p3))
print("heading4: {}, proximity4: {}".format(h4, p4))
      

Heading is in the range [-25 - 25], negative values stand for left, 0 for straight, positive for right side. Proximity is in the range [0 - 100] and measures in cm. The operation reads all 4 channels, two values per channel. The output of this program (seeker channel was 2):

heading1: 0, proximity1: -2147483648
heading2: -21, proximity2: 27
heading3: 0, proximity3: -2147483648
heading4: 0, proximity4: -2147483648
      

The beacon was placed front left of the infrared sensor, the distance was 27 cm. The channels 1, 3 and 4 returned a proximity value -2147483648, which is 0x|00:00:00:80| (little endian, the highest bit is 1, all the others are 0) and stands for no signal.

PID Controller

A PID controller continuously calculates an error value as the difference between a desired setpoint and a measured process variable. The controller attempts to minimize the error over time by adjustment of a control variable. This is a great algorithm to vary a parameter of a process until the process reaches its target state. The best is, that you need not know the exact dependencies of your parameter and the state of the process. A classic example is a radiator that heats a room. The process variable is the room temperature, the controller varies the position of the radiators valve until the room temperature is stable at the setpoint. We will use PID controllers to adjust the parameters speed and turn of a vehicles movement. We add a class PID to module ev3:

class PID():
    """
    object to implement a PID controller
    """
    def __init__(self,
                 setpoint: float,
                 gain_prop: float,
                 gain_der: float=None,
                 gain_int: float=None,
                 half_life: float=None
    ):
        self._setpoint = setpoint
        self._gain_prop = gain_prop
        self._gain_int = gain_int
        self._gain_der = gain_der
        self._half_life = half_life
        self._error = None
        self._time = None
        self._int = None
        self._value = None

    def control_signal(self, actual_value: float) -> float:
        if self._value is None:
            self._value = actual_value
            self._time = time.time()
            self._int = 0
            self._error = self._setpoint - actual_value
            return self._gain_prop * self._error
        else:
            time_act = time.time()
            delta_time = time_act - self._time
            self._time = time_act
            if self._half_life is None:
                self._value = actual_value
            else:
                fact1 = math.log(2) / self._half_life
                fact2 = math.exp(-fact1 * delta_time)
                self._value = fact2 * self._value + actual_value * (1 - fact2)
            error = self._setpoint - self._value
            if self._gain_int is None:
                signal_int = 0
            else:
                self._int += error * delta_time
                signal_int = self._gain_int * self._int
            if self._gain_der is None:
                signal_der = 0
            else:
                signal_der = self._gain_der * (error - self._error) / delta_time
            self._error = error
            return self._gain_prop * error + signal_int + signal_der
      

This implements a PID controller with one modification: half_life. The actual values may be noisy or change by descrete steps, we smooth them because the derivative part will show peaks, when the actual value changes randomly or descrete. half_life has the dimension of time [s] and is the half-life of the damping. But keep in mind: smoothing the controller makes it inert!

Its documentation:

    class PID(builtins.object)
     |  object to implement a PID controller
     |  
     |  Methods defined here:
     |  
     |  __init__(self, setpoint:float, gain_prop:float, gain_der:float=None, gain_int:float=None, half_life:float=None)
     |      Parametrizes a new PID controller
     |      
     |      Arguments:
     |      setpoint: ideal value of the process variable
     |      gain_prop: proportional gain,
     |                 high values result in fast adaption, but too high values produce oscillations or instabilities
     |      
     |      Keyword Arguments:
     |      gain_der: gain of the derivative part [s], decreases overshooting and settling time
     |      gain_int: gain of the integrative part [1/s], eliminates steady-state error, slower and smoother response
     |      half_life: used for discrete or noisy systems, smooths actual values [s]
     |  
     |  control_signal(self, actual_value:float) -> float
     |      calculates the control signal from the actual value
     |      
     |      Arguments:
     |      actual_value: actual measured process variable (will be compared to setpoint)
     |      
     |      Returns:
     |      control signal, which will be sent to the process
      

Keep in focus

Please place the infrared sensor onto your vehicle looking horizontally to the front. Plug it into port 2, select the beacons channel 1, activate the beacon, then start this program:

#!/usr/bin/env python3

import ev3, ev3_vehicle, struct

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,       # radius_wheel
    0.1175,        # tread
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
ops_read = b''.join([
    ev3.opInput_Device,
    ev3.READY_RAW,
    ev3.LCX(0),    # LAYER
    ev3.LCX(1),    # NO
    ev3.LCX(33),   # TYPE - IR
    ev3.LCX(1),    # MODE - Seeker
    ev3.LCX(2),    # VALUES
    ev3.GVX(0),    # VALUE1 - heading   channel 1
    ev3.GVX(4)     # VALUE2 - proximity channel 1
])
speed_ctrl = ev3.PID(0, 2, half_life=0.1, gain_der=0.2)
while True:
    reply = vehicle.send_direct_cmd(ops_read, global_mem=8)
    (heading, proximity) = struct.unpack('2i', reply[5:])
    if proximity == -2147483648:
        print("**** lost connection ****")
        break
    turn = 200
    speed = round(speed_ctrl.control_signal(heading))
    speed = max(-100, min(100, speed))
    vehicle.move(speed, turn)
vehicle.stop()
      

Remarks:

• We selceted channel 1, this allows to read this channels values only.
• The controller is not a PID, its a PD with smoothed values.
• If you move the beacon, your vehicle changes its orientation and keeps the beacon in the focus of its eyes.
• The program stops, when you switch off the beacon.
• The heading is the process variable, its setpoint is value 0 (straight). The adjustment is done by turning the vehicle on place.
• Please vary the parameters of the PD controller to get a feeling for the control mechanism.
• There is no steady-state error because control_signal == 0 holds the process in its stable state and is its only stable state.

Follow me

We slightly change the code of our program, but change its meaning fundamentally:

#!/usr/bin/env python3

import ev3, ev3_vehicle, struct

vehicle = ev3_vehicle.TwoWheelVehicle(
    0.02128,       # radius_wheel
    0.1175,        # tread
    protocol=ev3.BLUETOOTH,
    host='00:16:53:42:2B:99'
)
ops_read = b''.join([
    ev3.opInput_Device,
    ev3.READY_RAW,
    ev3.LCX(0),    # LAYER
    ev3.LCX(1),    # NO
    ev3.LCX(33),   # TYPE - IR
    ev3.LCX(1),    # MODE - Seeker
    ev3.LCX(2),    # VALUES
    ev3.GVX(0),    # VALUE1 - heading   channel 1
    ev3.GVX(4)     # VALUE2 - proximity channel 1
])
speed_ctrl =  ev3.PID(10, 4, half_life=0.1, gain_der=0.2)
turn_ctrl =  ev3.PID(0, 8, half_life=0.1, gain_der=0.3)
while True:
    reply = vehicle.send_direct_cmd(ops_read, global_mem=8)
    (heading, proximity) = struct.unpack('2i', reply[5:])
    if proximity == -2147483648:
        print("**** lost connection ****")
        break
    turn = round(turn_ctrl.control_signal(heading))
    turn = max(-200, min(200, turn))
    speed = round(-speed_ctrl.control_signal(proximity))
    speed = max(-100, min(100, speed))
    vehicle.move(speed, turn)
vehicle.stop()
      

This program uses the heading to control the movements argument turn and proximity to control its speed. The setpoint of the speed_ctrl is a distance (10 cm). If the distance grows, the controller increases the speed of the vehicle. You can reduce the distance under 10 cm, then the vehicle moves backwards. The controller always tries to hold or reach a distance of 10 cm between the beacon and the infrared sensor. Please vary the parameters of both controllers.

What happens if the beacon steadily moves forwards and the vehicle follows the beacon? This is like driving in a convoy and allows to study the steady-state error. Then speed = gain_prop * error resp. speed = gain_prop * (proximity - setpoint). This says: proximity = speed / gain_prop + setpoint. The stable distance between beacon and sensor grows with increasing speed from 10 cm (speed == 0) to 35 cm (speed == 100). If we simulate a convoy of vehicles, this is exactly what we want.

We can set gain_int to a positive value. Even very small values will eliminate the steady-state error. The convoy will hold a distance of 10 cm, even at high speed.

Conclusion

This lesson was about sensor values. We have seen, that motors are sensors too, which allow us to read the actual motor position. We wrote some little programs, that use the infrared sensor to control the movement of a vehicle with two drived wheels. These were our first real robot programs. Machines, that read sensor values, which allow to react on their environment.

We got some experience with PID controllers, which are the industry standard for controlled processes. Tuning their parameters replaces the coding of sophisticated algorithms. Our programs, that used PID controllers were astonishing compact and astonishing uniform. PID controllers seem to be powerfull and universal.

The next lesson will improve our class TwoWheelVehicle and prepare it for multitasking. I look forward to meet you again.