## Friday, 29 January 2016

### Lesson 4 - Exact movements of vehicles with two drived wheels

EV3 Direct commands - Lesson 04

## Introduction

Last lesson, we coded class `TwoWheelVehicle`, a subclass of `EV3`. Its methods are `move` and `stop`, but this is not more than a thin wrapper around the operations `opOutput_Speed`, `opOutput_Start` and `opOutput_Stop`. At the end of this lesson, class `TwoWheelVehicle` will have real substance. As most software, it grows step by step and this lesson will not be the last time, we work on this class.

Last lessons topic was a remote controlled vehicle. We coded unlimited movements, which were interrupted by new commands. We have seen the benefit of this design concept, it doesn't block the `EV3` device. The first version of exact movements (the result of this lesson) will block the `EV3` device and it will cost us further work to find a solution that doesn't block.

## Synchronized motor movements

Hopefully last lessons vehicle still exists. We need it once more. As you remember, the right wheel was connected to port A, the left one to port D. Our solution of the remote control had a deficit: the slower the movement, the worse became the precision of turns. What we need, is an operation, where the speed of two motors can be set to a defined ratio. As you may expect, this kind of operation exists. Please send the following direct command to your vehicle:

``````
-------------------------------------------------------------
\ len \ cnt \ty\ hd  \op\la\no\sp\ tu  \ step   \br\op\la\no\
-------------------------------------------------------------
0x|12:00|2A:00|80|00:00|B0|00|09|3B|81:32|82:D0:02|00|A6|00|09|
-------------------------------------------------------------
\ 18  \ 42  \no\ 0,0 \O \0 \A \-5\ 50  \ 720    \0 \O \0 \A \
\     \     \  \     \u \  \+ \  \     \        \  \u \  \+ \
\     \     \  \     \t \  \D \  \     \        \  \t \  \D \
\     \     \  \     \p \  \  \  \     \        \  \p \  \  \
\     \     \  \     \u \  \  \  \     \        \  \u \  \  \
\     \     \  \     \t \  \  \  \     \        \  \t \  \  \
\     \     \  \     \_ \  \  \  \     \        \  \_ \  \  \
\     \     \  \     \S \  \  \  \     \        \  \S \  \  \
\     \     \  \     \t \  \  \  \     \        \  \t \  \  \
\     \     \  \     \e \  \  \  \     \        \  \a \  \  \
\     \     \  \     \p \  \  \  \     \        \  \r \  \  \
\     \     \  \     \_ \  \  \  \     \        \  \t \  \  \
\     \     \  \     \S \  \  \  \     \        \  \  \  \  \
\     \     \  \     \y \  \  \  \     \        \  \  \  \  \
\     \     \  \     \n \  \  \  \     \        \  \  \  \  \
\     \     \  \     \c \  \  \  \     \        \  \  \  \  \
-------------------------------------------------------------
``````
Motor A rotates 720°, motor D moves 360°. The vehicle turns left, both motors rotate with low speed and well synchronized. The new operation is:
• `opOutput_Step_Sync = 0xB0` with the arguments:
• LAYER
• NOS: Two output ports, the command is not symmetric, it distinguishes between the lower port and the higher. In our case, the lower port is PORT_A, the right wheel, the higher is PORT_D, the left wheel.
• SPEED
• TURN: Turn ratio, [-200 - 200]
• STEP: Tacho pulses in degrees, value 0 stands for infinite movement (if TURN > 0, STEP limits the lower port, if TURN < 0, it limits the higher port). Positive, the sign of argument SPEED makes the direction
• BRAKE

The argument TURN needs some explanations. But you are in a good position, you already know its meaning, because we used it in our remote control program of lesson 3. As you may remember, we calculated the speed of the two wheels as:

``````
if turn > 0:
speed_right  = speed
speed_left = round(speed * (1 - turn / 100))
else:
speed_right  = round(speed * (1 + turn / 100))
speed_left = speed
``````
The rounding to integer values was the cause of the bad precision at low speed! But now we become happy, operation `opOutput_Step_Sync` does the calculation without rounding:
``````
if turn > 0:
speed_right  = speed
speed_left = speed * (1 - turn / 100)
else:
speed_right  = speed * (1 + turn / 100)
speed_left = speed
``````
Speed and steps are proportional, we can also write:
``````
if turn > 0:
step_right  = math.copysign(1, speed) * step
step_left = math.copysign(1, speed) * step * (1 - turn / 100)
else:
step_right  = math.copysign(1, speed) * step * (1 + turn / 100)
step_left = math.copysign(1, speed) * step
``````
Operation `opOutput_Step_Sync` is perfect for vehicles with two drived wheels! It seems, as if it is made for it. Please improve your remote control program by replacing the two operations `opOutput_Speed` with a single `opOutput_Step_Sync`. You will see, it works better, especially at low speed. I changed my code of function `move` to:
``````
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 = 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
])
self.send_direct_cmd(ops)
``````

A few remarks:

• The vehicle follows the same turn, independent from speed. This is the main improvement of `opOutput_Step_Sync` compared with the usage of two operations `opOutput_Speed`.
• If you used `opOutput_Polarity`, you will realize, that you can't combine it with `opOutput_Step_Sync`. You have to do it by hand. Changing the polarity of both motors is easy, just invert argument SPEED.
• If you interchanged the connection of the motors, so that your left motor is connected to the lower port, it's easy too, invert TURN.
• Changing the polarity of only one motor is tricky.

The following table may help to order the movements you have still seen. It describes the type of movement dependent from TURN, when the following conditions are given:

• both motors have the same polarity,
• the right wheel is connected to the lower port, the left to the higher.
TURN movement description
0 straight Both motors move in same direction and with same speed.
[0 - 100] turn left Both motors rotate in same direction, the left one moves with lower speed.
100 turn left around the left wheel Only the right motor moves.
[100 - 200] narrow turn left Both motors rotate in opposite direction, the left one rotates with lower speed.
200 circle left Both motors move in opposite direction, but same speed.
[0 - -100] turn right Both motors rotate in same direction, the right one moves with lower speed.
-100 turn right around the right wheel Only the left motor moves.
[-100 - -200] narrow turn right Both motors rotate in opposite direction, the right one moves with lower speed.
-200 circle right Both motors move in opposite direction, but same speed.

Please reflect the situation, when the ports are differently connected, when the left wheel is the lower port and the right one the higher.

You are also welcome, for further improvements of the remote control project. You can use a joystick instead of your keyboards arrow-keys. Or you can take the gyro sensor of your smartphone. But this is your project, we left the remote control behind (at least for the moment).

## Well defined and predictable movements of vehicles with two drived wheels

We still focuse on vehicles with two drived wheels and the exactness of their movements. A remote control is a very special situation, where a human mind supervises the movement of the vehicle and immediately does some corrections, if this is needed. This situation does not need units like the radius of a turn. It's like driving a car, there is no need to know exactly how the radius of a turn depends from the position of the steering wheel. The corrections are relative and intuitive. Robots move without external control and their algorithm needs to know the exact dependencies of the parameters. We will write programs, which control the movement of a vehicle. We start with the hardest variant, where no mechanism of corrections exists. This means, we need functions, that predictable and precise describe the movement of the vehicle. I think of the following:

• ```drive_straight(speed:int, distance: float=None)```, where
• the sign of `speed` describes the direction (forward or backward).
• the absolute value of `speed` describes the velocity. We would prefer the SI unit meter per second, but it's in percent.
• the `distance` is None or positive, it is given in the SI unit `meter`. If it is None, the movement is unlimited.
• ```drive_turn(speed:int, radius_turn:float, angle:float=None, right_turn:bool=False)```, where
• the sign of `speed` describes the direction (forward or backward).
• the absolute value of `speed` describes the velocity.
• `radius_turn` is the radius of the turn in `meter`. We take the middle between the two drived wheels as our point of reference.
• the sign of `radius_turn` decribes the direction of the turn. Positive values for left turns, which is the positive rotation direction, negative values stand for clockwise turns.
• `angle` is None or positive, it is the circle segment in `degrees` (90° is a quarter circle, 720° are two whole circles). If it is None, an unlimited movement is meant.
• `right_turn` is a flag for a very special situation. If we turn on place, attribute `radius_turn` is zero and has no sign. In this case, turning left is the default and attribute `right_turn` is the flag for the opposite direction.
As you may imagine, we are not so far away from that. We will use the operations `opOutput_Ready`, `opOutput_Start` and `opOutput_Speed_Sync`. This says we will not use interruption. From this point of view we fall back to the knowledge of lesson 2.

### Determine the dimensions of your vehicle

We need some dimensions of the vehicle to translate the SI units of the above described functions into the arguments `turn` and `step` of operation `opOutput_Speed_Sync`. The dimensions of the vehicle are:

• Radius of the drived wheels: `radius_wheel`.
• The vehicles `tread`.

Using a yardstick gives (for my vehicle):

• `radius_wheel = 0.021 m`
• `tread = 0.16 m`

An alternative with better accuracy is the measurement of the vehicles movement. To get the radius of the drived wheels, you can use the following direct command:

``````
----------------------------------------------------------
\ len \ cnt \ty\ hd  \op\la\no\sp\tu\ step   \br\op\la\no\
----------------------------------------------------------
0x|11:00|2A:00|80|00:00|B0|00|09|14|00|82:10:0E|01|A6|00|09|
----------------------------------------------------------
\ 17  \ 42  \no\ 0,0 \O \0 \A \20\0 \ 3600   \1 \O \0 \A \
\     \     \  \     \u \  \+ \  \  \        \  \u \  \+ \
\     \     \  \     \t \  \D \  \  \        \  \t \  \D \
\     \     \  \     \p \  \  \  \  \        \  \p \  \  \
\     \     \  \     \u \  \  \  \  \        \  \u \  \  \
\     \     \  \     \t \  \  \  \  \        \  \t \  \  \
\     \     \  \     \_ \  \  \  \  \        \  \_ \  \  \
\     \     \  \     \S \  \  \  \  \        \  \S \  \  \
\     \     \  \     \t \  \  \  \  \        \  \t \  \  \
\     \     \  \     \e \  \  \  \  \        \  \a \  \  \
\     \     \  \     \p \  \  \  \  \        \  \r \  \  \
\     \     \  \     \_ \  \  \  \  \        \  \t \  \  \
\     \     \  \     \S \  \  \  \  \        \  \  \  \  \
\     \     \  \     \y \  \  \  \  \        \  \  \  \  \
\     \     \  \     \n \  \  \  \  \        \  \  \  \  \
\     \     \  \     \c \  \  \  \  \        \  \  \  \  \
----------------------------------------------------------
``````
Take your yardstick and measure the `distance` of your vehicles movement (if your vehicle does not drive straight, look for the best combination of wheels, not everthing, that seems to be of identical size, really is). The `distance` of one full rotation of a wheel calculates as `2 * pi * radius_wheel`. 3,600 degrees are 10 full rotations, so the following calculation gives the `radius_wheel` of your wheels:
``````
radius_wheel = distance / (20 * pi)
``````
This corrected my wheels radius to `radius_wheel = 0.02128 m`. Next, we let the vehicle circle on place and count `N`, the number of the vehicles rotations. To do so, we send the direct command:
``````
----------------------------------------------------------------
\ len \ cnt \ty\ hd  \op\la\no\sp\ tu     \ step   \br\op\la\no\
----------------------------------------------------------------
0x|13:00|2A:00|80|00:00|B0|00|09|14|82:C8:00|82:50:46|01|A6|00|09|
----------------------------------------------------------------
\ 19  \ 42  \no\ 0,0 \O \0 \A \20\ 200    \ 18000  \1 \O \0 \A \
\     \     \  \     \u \  \+ \  \        \        \  \u \  \+ \
\     \     \  \     \t \  \D \  \        \        \  \t \  \D \
\     \     \  \     \p \  \  \  \        \        \  \p \  \  \
\     \     \  \     \u \  \  \  \        \        \  \u \  \  \
\     \     \  \     \t \  \  \  \        \        \  \t \  \  \
\     \     \  \     \_ \  \  \  \        \        \  \_ \  \  \
\     \     \  \     \S \  \  \  \        \        \  \S \  \  \
\     \     \  \     \t \  \  \  \        \        \  \t \  \  \
\     \     \  \     \e \  \  \  \        \        \  \a \  \  \
\     \     \  \     \p \  \  \  \        \        \  \r \  \  \
\     \     \  \     \_ \  \  \  \        \        \  \t \  \  \
\     \     \  \     \S \  \  \  \        \        \  \  \  \  \
\     \     \  \     \y \  \  \  \        \        \  \  \  \  \
\     \     \  \     \n \  \  \  \        \        \  \  \  \  \
\     \     \  \     \c \  \  \  \        \        \  \  \  \  \
----------------------------------------------------------------

``````
I counted `N = 15.2` full rotations of my vehicle. Both wheels rotated 18.000°, which are 50 full rotations, or the distance `50 * 2 * pi * radius_wheel`. The radius of the turn was `0.5 * tread` (we defined the middle between the wheels as our point of reference). This says: `N * 2 * pi * 0.5 * tread = 50 * 2 * pi * radius_wheel` or:
``````
tread = radius_wheel * 100 / N
``````
This corrects the dimension of `tread` (in my case: `tread = 0.1346 m`). Later we will do some additional movements, maybe this will correct the dimension `tread` once more.

### Mathematical transformations to get the arguments STEP and TURN

Ok, we know our vehicles dimensions. Next we need to translate the arguments of our methods `drive_straight` and `drive_turn` into the arguments STEP and TURN of operation `opOutput_Step_Sync`. This needs some mathematics. If you are not interested in the details, you can just take the result from the bottom of this section. But most of you want to know the details, here they are.

Given is `angle` and `radius_turn` of the turn, our vehicle has to do. In a turn, the two wheels move different distances. The distance of the outer wheel is: `2 * pi * radius_wheel * STEP / 360`. The same distance can be calculated from the geometrie of the turn and the knowledge of the vehicles tread: `2 * pi * (radius_turn + 0.5 * tread) * angle / 360`. This two descriptions of the same distance give the following equation:

``````
STEP = angle * (radius_turn + 0.5 * tread) / radius_wheel
``````
Ok, the first argument is calculated, but we still need the calculation of TURN. The crucial approach comes from the geometry of the turn and says, that the ratio between the two wheels speed `speed_right / speed_left` is the same as the ratio between the moved distances of the outer and the inner wheel:
``````
speed_right / speed_left = (radius_turn + 0.5 * tread) / (radius_turn - 0.5 * tread)
``````
As you may remember, we already know the speeds of the two wheels: `speed_right = SPEED` and `speed_left = SPEED * (1 - TURN / 100)`. This gives the equation:
``````
1 / (1 - TURN / 100) = (radius_turn + 0.5 * tread) / (radius_turn - 0.5 * tread)
``````
The transformation of this equation results in:
``````
TURN  = 100 * (1 - (radius_turn - 0.5 * tread) / (radius_turn + 0.5 * tread))
``````
This was it, if the dimensions of the turn movement (`angle`, `radius_turn`) and the dimensions of the vehicle (`tread`, `radius_wheel`) are given:
1. variable STEP calculates as:
``````
STEP = angle * (radius_turn + 0.5 * tread) / radius_wheel
``````
2. variable TURN calculates as:
``````
TURN = 100 * (1 - (radius_turn - 0.5 * tread) / (radius_turn + 0.5 * tread))
``````
In python, this reads as:
``````
turn = round(100*(1 - rad_inner / rad_outer))
if angle < 0:
step *= -1
turn *= -1
if self._polarity == -1:
speed *= -1
``````

### Control of the mathematical transformations

Let's do some plausibility checks:

• Turns with radius `radius_turn = 0.5 * tread` result in `TURN = 100` or `TURN = -100`, this is correct.
• Turns with radius `radius_turn = 0` result in `TURN = 200` or `TURN = -200`, also ok.

Now, we come to a real test. A turn with `angle = 90°` and `radius_turn = 0.5 m`. In my case the calculation above gives `STEP = 2358` and `TURN = 23`, which results in the command:

``````
----------------------------------------------------------
\ len \ cnt \ty\ hd  \op\la\no\sp\tu\ step   \br\op\la\no\
----------------------------------------------------------
0x|11:00|2A:00|80|00:00|B0|00|09|14|17|82:36:09|01|A6|00|09|
----------------------------------------------------------
\ 17  \ 42  \no\ 0,0 \O \0 \A \20\23\ 2358   \1 \O \0 \A \
\     \     \  \     \u \  \+ \  \  \        \  \u \  \+ \
\     \     \  \     \t \  \D \  \  \        \  \t \  \D \
\     \     \  \     \p \  \  \  \  \        \  \p \  \  \
\     \     \  \     \u \  \  \  \  \        \  \u \  \  \
\     \     \  \     \t \  \  \  \  \        \  \t \  \  \
\     \     \  \     \_ \  \  \  \  \        \  \_ \  \  \
\     \     \  \     \S \  \  \  \  \        \  \S \  \  \
\     \     \  \     \t \  \  \  \  \        \  \t \  \  \
\     \     \  \     \e \  \  \  \  \        \  \a \  \  \
\     \     \  \     \p \  \  \  \  \        \  \r \  \  \
\     \     \  \     \_ \  \  \  \  \        \  \t \  \  \
\     \     \  \     \S \  \  \  \  \        \  \  \  \  \
\     \     \  \     \y \  \  \  \  \        \  \  \  \  \
\     \     \  \     \n \  \  \  \  \        \  \  \  \  \
\     \     \  \     \c \  \  \  \  \        \  \  \  \  \
----------------------------------------------------------
``````
Indeed, my vehicle moved a nearly perfect quarter of a circle with a radius of `0.5 m`!

## Enhance class TwoWheelVehicle

Enough mathematics, at least for the moment, let's code now! As our first task, we modify the constructor of class `TwoWheelVehicle`. We add the two dimensions `radius_wheel` and `tread`, which were identified as required:

``````
def __init__(
self,
protocol: str=None,
host: str=None,
ev3_obj: ev3.EV3=None
):
super().__init__(protocol=protocol, host=host, ev3_obj=ev3_obj)
self._polarity = 1
self._port_left = ev3.PORT_D
self._port_right = ev3.PORT_A
``````

Next we code method `_drive`, which is very close to method `move`. It's called with the arguments `speed`, `turn` and `step`. The outside world thinks in `radius_turn` and `angle`, which must be translated into the internal arguments `turn` and `step`. This says, methods `drive_straight` and `drive_turn` do this translation, then they call the internal method `_drive`:

``````
def _drive(self, speed: int, turn: int, step: int) -> bytes:
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]"
if self._polarity == -1:
speed *= -1
if self._port_left < self._port_right:
turn *= -1
ev3.LCX(0),                                  # LAYER
ev3.LCX(self._port_left + self._port_right)  # NOS
])
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(step),
ev3.LCX(0),                                  # BRAKE
ev3.opOutput_Start,
ev3.LCX(0),                                  # LAYER
ev3.LCX(self._port_left + self._port_right)  # NOS
])
if self._sync_mode == ev3.SYNC:
return self.send_direct_cmd(ops_start + ops_ready)
else:
return self.send_direct_cmd(ops_ready + ops_start)
``````
We distinguish between `SYNC` and `ASYNC` or `STD`. In case of `ASYNC` or `STD`, we wait before the movement starts, in case of `SYNC` until it's finished. If you did download module `ev3_vehicle.py` from ev3-python3, you will not find a method `_drive`. This is a hint, that we will come back to class `TwoWheelVehicle` to implement interruption. We code method `drive_turn`:
``````
def drive_turn(
self,
speed: int,
angle: float=None,
right_turn: bool=False
) -> None:
assert isinstance(radius_turn, numbers.Number), "radius_turn needs to be a number"
assert angle is None or isinstance(angle, numbers.Number), "angle needs to be a number"
assert angle is None or angle > 0, "angle needs to be positive"
assert isinstance(right_turn, bool), "right_turn needs to be a boolean"
if radius_turn >= 0 and not right_turn:
turn = round(100*(1 - rad_left / rad_right))
else:
turn = - round(100*(1 - rad_right / rad_left))
if turn == 0:
raise ValueError("radius_turn is too large")
if angle is None:
self.move(speed, turn)
else:
if turn > 0:
else:
self._drive(speed, turn, step)
``````
Very large values of `radius_turn` cause problems. After rounding to integers, they result in straight movements. In this case we raise an error.

Method `drive_straight`:

``````
def drive_straight(self, speed: int, distance: float=None) -> None:
assert distance is None or isinstance(distance, numbers.Number), \
"distance needs to be a number"
assert distance is None or distance > 0, \
"distance needs to be positive"
if distance is None:
self.move(speed, 0)
else:
step = round(distance * 360 / (2 * math.pi * self._radius_wheel))
self._drive(speed, 0, step)
``````
Relax, we have realized a tool, that drives vehicles predictable. Please do some tests!

## Knowledge of the vehicles position and orientation

Sorry, a few sentences ago i wrote enough mathematics, and now I come with trigonometry. But we are in a situation, where it's really worth the effort. Imagine, you drive your vehicle and after a series of movements, you need to know its position and its orientation. We will do that without any usage of sensors. Instead we use pure mathematics and no magic.

Let's make some assumptions:

• The position, where your vehicle is placed, when you create the class `TwoWheelVehicle` will be the origin of your coordinate system (0, 0).
• The direction, which in this moment points straight forward, is the direction of the x-axis.
• The y-axis directs to the left hand side of your vehicle.
• The position of your vehicle is described as x- and y-coordinates. It's in meter.
• The `orientation` of your vehicle is the difference between the original orientation of the vehicle and its actual orientation. It's in degrees. Left turns increase the orientation, right turns decrease it.

This time, I will not present the mathematics. Take it as it is, or take it as a riddle, you have to solve. But please add the following logic to your class `TwoWheelVehicle`:

• constructor:
``````
self._orientation = 0.0
self._pos_x = 0.0
self._pos_y = 0.0
``````
• `drive_straight(speed, distance)`
``````
diff_x = distance * math.cos(math.radians(self._orientation))
diff_y = distance * math.sin(math.radians(self._orientation))
if speed > 0:
self._pos_x += diff_x
self._pos_y += diff_y
else:
self._pos_x -= diff_x
self._pos_y -= diff_y
``````
• `drive_turn(speed, radius_turn, angle)`
``````
angle += 180
angle %= 360
angle -= 180
fact = 2.0 * radius_turn * math.sin(math.radians(0.5 * angle))
self._orientation += 0.5 * angle
self._pos_x += fact * math.cos(math.radians(self._orientation))
self._pos_y += fact * math.sin(math.radians(self._orientation))
self._orientation += 0.5 * angle
self._orientation += 180
self._orientation %= 360
self._orientation -= 180
``````

## Complete class TwoWheelVehicle

We complete class `TwoWheelVehicle` with some more functionality:

• We add a method `rotate_to(speed: int, o: float)`, that does the following:
• calculates the distance between the actual and the new orientation.
• calls `drive_turn` with `radius_turn = 0` to rotate the vehicle, so that it gets the new orientation.
• We add a method `drive_to(self, speed: int, x: float, y: float)`, that does the following:
• calculates the distance between the actual position and the new position:
``````
diff_x = pos_x - self._pos_x
diff_y = pos_y - self._pos_y
``````
We need it in coordinates and as an absolute value:
``````
distance = math.sqrt(diff_x**2 + diff_y**2)
``````
• calculates the direction to the new position. This is tricky, you need a thorough understanding of trigonometry, because you have to use `atan`, the inverse function of `tan`. I give you a hint:
``````
if abs(diff_x) > abs(diff_y):
direction = math.degrees(math.atan(diff_y/diff_x))
else:
fract = diff_x / diff_y
sign = math.copysign(1.0, fract)
direction = sign * 90 - math.degrees(math.atan(fract))
if diff_x < 0:
direction += 180
direction %= 360
``````
• calls `rotate_to`, so that the `orientation` points to `direction`.
• calls `drive_straight` to move the vehicle to the new position.

We do some tests:

• We send the vehicle to some circular trips and code series of `drive_to`, which end at `position = (0,0)`. We add a final `rotate_to` with `orientation = 0`. Please evaluate, if the vehicle really returns to its original position and orientation.
• We add some `drive_turn` to the circular trip and evaluate, if the error of `drive_turn` is larger or smaller than that of `drive_to`.

Turns with large `radius_turn` have a bad precision. Again this is a result of rounding. In this case, the rounding of TURN to an integer value creates the error.

If you downloaded module `ev3_vehicle` from ev3-python3, you need to add a final call of method `stop` when you call its methods `drive_straight`, `drive_turn`, `rotate_to` or `drive_to`!

## Asynchronous and synchronous movements

Let's take a closer look to the driving of our vehicle. Here's my program with a circular trip:

``````
#!/usr/bin/env python3

import ev3, ev3_vehicle

my_vehicle = ev3_vehicle.TwoWheelVehicle(0.02128, 0.1346, protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
my_vehicle.verbosity = 1
speed = 25
my_vehicle.drive_straight(speed, 0.05)
my_vehicle.drive_turn(speed, -0.07, 65)
my_vehicle.drive_straight(speed, 0.35)
my_vehicle.drive_turn(speed, 0.20, 140)
my_vehicle.drive_straight(speed, 0.15)
my_vehicle.drive_turn(speed, -1.10, 55)
my_vehicle.drive_turn(speed, 0.35, 160)
my_vehicle.drive_to(speed, 0.0, 0.0)
my_vehicle.rotate_to(speed, 0.0)
``````
The output of this program:
``````
15:42:05.989592 Sent 0x|14:00|2A:00|80|00:00|AA:00:09:B0:00:09:19:00:82:87:00:00:A6:00:09|
15:42:05.990443 Sent 0x|15:00|2B:00|80|00:00|AA:00:09:B0:00:09:19:81:9E:82:A3:01:00:A6:00:09|
15:42:05.990925 Sent 0x|14:00|2C:00|80|00:00|AA:00:09:B0:00:09:19:00:82:AE:03:00:A6:00:09|
15:42:05.991453 Sent 0x|15:00|2D:00|80|00:00|AA:00:09:B0:00:09:19:81:32:82:DF:06:00:A6:00:09|
15:42:05.991882 Sent 0x|14:00|2E:00|80|00:00|AA:00:09:B0:00:09:19:00:82:94:01:00:A6:00:09|
15:42:05.992330 Sent 0x|14:00|2F:00|80|00:00|AA:00:09:B0:00:09:19:34:82:C9:0B:00:A6:00:09|
15:42:05.992766 Sent 0x|15:00|30:00|80|00:00|AA:00:09:B0:00:09:19:81:20:82:42:0C:00:A6:00:09|
15:42:05.993306 Sent 0x|16:00|31:00|80|00:00|AA:00:09:B0:00:09:19:82:38:FF:82:D0:01:00:A6:00:09|
15:42:05.993714 Sent 0x|14:00|32:00|80|00:00|AA:00:09:B0:00:09:19:00:82:3F:03:00:A6:00:09|
15:42:05.994202 Sent 0x|15:00|33:00|80|00:00|AA:00:09:B0:00:09:19:82:38:FF:81:69:00:A6:00:09|
``````
Within five milliseconds the program sends all direct commands to the `EV3` device, where they are queued and wait to be operated. This is simple to code, but blocks the `EV3` device until the last of the commands is started. Please follow the code and reflect, which values the properties `pos_x`, `pos_y` and `orientation` have and if they correspond to the real position and orientation of our vehicle.

This is asynchronous behaviour! The program and the `EV3` device act on different timescales.

Now we change to synchronous mode and compare the behaviour:

``````
#!/usr/bin/env python3

import ev3, ev3_vehicle

my_vehicle = ev3_vehicle.TwoWheelVehicle(0.02128, 0.1346, protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
my_vehicle.verbosity = 1
speed = 25
my_vehicle.sync_mode = ev3.SYNC
my_vehicle.drive_straight(speed, 0.05)
my_vehicle.drive_turn(speed, -0.07, 65)
my_vehicle.drive_straight(speed, 0.35)
my_vehicle.drive_turn(speed, 0.20, 140)
my_vehicle.drive_straight(speed, 0.15)
my_vehicle.drive_turn(speed, -1.10, 55)
my_vehicle.drive_turn(speed, 0.35, 160)
my_vehicle.drive_to(speed, 0.0, 0.0)
my_vehicle.rotate_to(speed, 0.0)
``````
This version produced the following output:
``````
15:46:19.859532 Sent 0x|14:00|2A:00|00|00:00|B0:00:09:19:00:82:87:00:00:A6:00:09:AA:00:09|
15:46:20.307045 Recv 0x|03:00|2A:00|02|
15:46:20.307760 Sent 0x|15:00|2B:00|00|00:00|B0:00:09:19:81:9E:82:A3:01:00:A6:00:09:AA:00:09|
15:46:22.100007 Recv 0x|03:00|2B:00|02|
15:46:22.100612 Sent 0x|14:00|2C:00|00|00:00|B0:00:09:19:00:82:AE:03:00:A6:00:09:AA:00:09|
15:46:24.597018 Recv 0x|03:00|2C:00|02|
15:46:24.597646 Sent 0x|15:00|2D:00|00|00:00|B0:00:09:19:81:32:82:DF:06:00:A6:00:09:AA:00:09|
15:46:29.141999 Recv 0x|03:00|2D:00|02|
15:46:29.142609 Sent 0x|14:00|2E:00|00|00:00|B0:00:09:19:00:82:94:01:00:A6:00:09:AA:00:09|
15:46:30.221994 Recv 0x|03:00|2E:00|02|
15:46:30.222626 Sent 0x|14:00|2F:00|00|00:00|B0:00:09:19:34:82:C9:0B:00:A6:00:09:AA:00:09|
15:46:44.779922 Recv 0x|03:00|2F:00|02|
15:46:44.780364 Sent 0x|15:00|30:00|00|00:00|B0:00:09:19:81:20:82:42:0C:00:A6:00:09:AA:00:09|
15:46:46.938901 Recv 0x|03:00|30:00|02|
15:46:46.939701 Sent 0x|16:00|31:00|00|00:00|B0:00:09:19:82:38:FF:82:D0:01:00:A6:00:09:AA:00:09|
15:46:55.695901 Recv 0x|03:00|31:00|02|
15:46:55.696508 Sent 0x|14:00|32:00|00|00:00|B0:00:09:19:00:82:3F:03:00:A6:00:09:AA:00:09|
15:47:05.061856 Recv 0x|03:00|32:00|02|
15:47:05.062504 Sent 0x|15:00|33:00|00|00:00|B0:00:09:19:82:38:FF:81:69:00:A6:00:09:AA:00:09|
15:47:05.097692 Recv 0x|03:00|33:00|02|
``````
The movement of the vehicle is the same, but now the program sends one direct command and waits until it's finished, then it sends the next one. `sync_mode = SYNC` works as designed, it synchronizes the timescales of the program and the `EV3` device. Another benefit is, that it controls the success per direct command and directly reacts, if something unexpected happens.

Both versions, asynchronous and synchronous, block the `EV3` device.

## Conclusion

Class `TwoWheelVehicle` controls the movement of a vehicle with two drived wheels. If we know the geometry of a course, we can code a program, that drives our vehicle through it.

We have seen the difference between synchronous and asynchronous mode. For the moment, we prefer `SYNC`. But what we target is a solution, which drives the vehicle synchronously and doesn't block the `EV3` device. This would open the door to multitasking.

My class `EV3TwoWheelVehicle` actually has the following state:

``````
Help on module ev3_vehicle:

NAME
ev3_vehicle - EV3 vehicle

CLASSES
ev3.EV3(builtins.object)
TwoWheelVehicle

class TwoWheelVehicle(ev3.EV3)
|  ev3.EV3 vehicle with two drived Wheels
|
|  Method resolution order:
|      TwoWheelVehicle
|      ev3.EV3
|      builtins.object
|
|  Methods defined here:
|
|  __init__(self, radius_wheel:float, tread:float, protocol:str=None, host:str=None, ev3_obj:ev3.EV3=None)
|      Establish a connection to a LEGO EV3 device
|
|      Arguments:
|      radius_wheel: radius of the wheels im meter
|      tread: the vehicles tread in meter
|
|      Keyword Arguments (either protocol and host or ev3_obj):
|      protocol
|        BLUETOOTH == 'Bluetooth'
|        USB == 'Usb'
|        WIFI == 'Wifi'
|      host: mac-address of the LEGO EV3 (f.i. '00:16:53:42:2B:99')
|      ev3_obj: an existing EV3 object (its connections will be used)
|
|  drive_straight(self, speed:int, distance:float=None) -> None
|      Drive the vehicle straight forward or backward.
|
|      Attributes:
|      speed: in percent [-100 - 100] (direction depends on its sign)
|          positive sign: forwards
|          negative sign: backwards
|
|      Keyword Attributes:
|      distance: in meter, needs to be positive
|                if None, unlimited movement
|
|  drive_to(self, speed:int, pos_x:float, pos_y:float) -> None
|      Drive the vehicle to the given position.
|
|      Attributes:
|      speed: in percent [-100 - 100] (direction depends on its sign)
|          positive sign: forwards
|          negative sign: backwards
|      x: x-coordinate of target position
|      y: y-coordinate of target position
|
|  drive_turn(self, speed:int, radius_turn:float, angle:float=None, right_turn:bool=False) -> None
|      Drive the vehicle a turn with given radius.
|
|      Attributes:
|      speed: in percent [-100 - 100] (direction depends on its sign)
|          positive sign: forwards
|          negative sign: backwards
|      radius_turn: in meter
|          positive sign: turn to the left side
|          negative sign: turn to the right side
|
|      Keyword Attributes:
|      angle: absolute angle (needs to be positive)
|             if None, unlimited movement
|      right_turn: flag of turn right (only in case of radius_turn == 0)
|
|  move(self, speed:int, turn:int) -> None
|      Start unlimited movement of the vehicle
|
|      Arguments:
|      speed: speed in percent [-100 - 100]
|        > 0: forward
|        < 0: backward
|      turn: type of turn [-200 - 200]
|        -200: circle right on place
|        -100: turn right with unmoved right wheel
|         0  : straight
|         100: turn left with unmoved left wheel
|         200: circle left on place
|
|  rotate_to(self, speed:int, orientation:float) -> None
|      Rotate the vehicle to the given orientation.
|      Chooses the direction with the smaller movement.
|
|      Attributes:
|      speed: in percent [-100 - 100] (direction depends on its sign)
|      orientation: in degrees [-180 - 180]
|
|  stop(self, brake:bool=False) -> None
|      Stop movement of the vehicle
|
|      Arguments:
|      brake: flag if activating brake
|
|  ----------------------------------------------------------------------
|  Data descriptors defined here:
|
|  orientation
|      actual orientation of the vehicle in degree, range [-180 - 180]
|
|  polarity
|      polarity of motor rotation (values: -1, 1, default: 1)
|
|  port_left
|      port of left wheel (default: PORT_D)
|
|  port_right
|      port of right wheel (default: PORT_A)
|
|  pos_x
|      actual x-component of the position in meter
|
|  pos_y
|      actual y-component of the position in meter
|
|  ----------------------------------------------------------------------
|  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).
``````

I hope, you got the feeling, that we are doing real things now. For me, it was a highlight. Only in rare cases, one can earn so much with so little effort.

#### 1 comment:

1. Excellent! Very helpful, sir. Danke schoen!