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:

        rad_outer = radius_turn + 0.5 * self._tread
        rad_inner = radius_turn - 0.5 * self._tread
        step = round(angle*rad_outer / self._radius_wheel)
        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,
            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
      

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
        ops_ready = b''.join([
            ev3.opOutput_Ready,
            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,
            radius_turn: float,
            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"
        rad_right = radius_turn + 0.5 * self._tread
        rad_left = radius_turn - 0.5 * self._tread
        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:
                step = round(angle*rad_right / self._radius_wheel)
            else:
                step = - round(angle*rad_left / self._radius_wheel)
            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!

    ReplyDelete