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.