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.