EV3 直接命令 - 第 4 课 用两个驱动轮精确地移动小车

发表于 | 分类于 | | 阅读次数:

简介

上一课，我们编写了 TwoWheelVehicle 类，一个 EV3 的子类。它的方法是 move 和 stop，但它不仅仅是围绕操作 opOutput_Speed，opOutput_Start 和 opOutput_Stop 的薄薄的封装。本节课的最后，类 TwoWheelVehicle 将具有实质的内容。如大多数软件那样，它一步步增长，本节课不会是最后一次，我们将继续使用这个类。

上一课的主题是一个遥控小车。我们编写了无限移动的代码，它们由新命令中断。我们已经看到了这一设计理念的好处，它不阻塞 EV3 设备。精确移动的第一个版本（本节课的结果）将阻塞 EV3 设备，且它将耗费我们进一步的工作来找到一个不阻塞的方案。

同步的电机移动

希望上一节课的车辆仍然存在。我们再次需要它。你还记得，右轮连接到端口 A，左边连接到端口 D。我们的遥控解决方案存在缺陷：移动越慢，转向的精度越差。我们需要的是一个操作，两个电机的速度可以被设置为定义的比率。如你可能期待的那样，这种操作是存在的。请给你的小车发送如下的直接命令：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
-------------------------------------------------------------                   
 \ 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 \  \  \  \     \        \  \  \  \  \  
                   -------------------------------------------------------------

点击 A 旋转 720°，点击 D 移动 360°。小车左转，两个电机都以低速转动且良好的同步。新的操作是：

  • opOutput_Step_Sync = 0xB0，参数是：
    • LAYER
    • NOS：两个输出端口，命令不是对称的，它区别了低端口和高端口。在我们的情况中，低端口是 PORT_A，右轮，高端口是 PORT_D，左轮。
    • SPEED
    • TURN：转向比率，[-200 - 200]
    • STEP：转速脉冲以度为单位，值 0 代表无限运动（如果TURN > 0，则 STEP 限制低端口，如果 TURN < 0，则限制较高端口）。正值，参数 SPEED 的符号确定方向。
    • BRAKE

参数 TURN 需要一些解释。但你处在一个很好的位置，你已经知道了它的含义，因为我们在第 3 课的远程控制程序中使用了它。如你可能记得的那样，我们计算两轮的速度：

1
2
3
4
5
6
if turn > 0:
    speed_right  = speed
    speed_left = round(speed * (1 - turn / 100))
else:
    speed_right  = round(speed * (1 + turn / 100))
    speed_left = speed

到整数值的舍入是低速下糟糕的精度的原因！但现在我们很高兴，操作 opOutput_Step_Sync 计算时不执行舍入：

1
2
3
4
5
6
if turn > 0:
    speed_right  = speed
    speed_left = speed * (1 - turn / 100)
else:
    speed_right  = speed * (1 + turn / 100)
    speed_left = speed

速度和步数是成比例的，我们也可以写为：

1
2
3
4
5
6
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

操作 opOutput_Step_Sync 对于具有两个驱动轮的小车是完美的！就像是专门为它设计的一样。请通过把两个 opOutput_Speed 操作替换为一个 opOutput_Step_Sync 来提升你的远程控制程序。你将看到，它工作的更好，特别是低速下。我把我的 move 函数的代码修改为了：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
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)

一些说明：

  • 车辆遵从相同的转弯，与速度无关。这是 opOutput_Step_Sync 相对于使用两个 opOutput_Speed 操作的主要提升。
  • 如果你使用了 opOutput_Polarity，你将发现 ，你无法把它和 opOutput_Step_Sync 结合。你不得不手动完成。修改两个电机的极性很简单，只是反转参数 SPEED 即可。
  • 如果你交换了电机的连接，以使你的左边电机连接在较低端口上，它也很简单，反转 TURN。
  • 仅更改一个电机的极性非常棘手。

下表可能有助于理清你已经看到的移动。当给出以下条件时，它描述了依赖于 TURN 的运动类型：

  • 两个电机具有相同的极性，
  • 右轮连接到更低的端口，左边的更高。
TURN 移动 描述
0 直行 两个电机以相同的速度朝相同的方向移动
[0 - 100] 左转 两个电机朝相同的方向旋转，左边的以更低的速度
100 绕左轮左转 只有右边的电机移动
[100 - 200] 向左转弯 两个电机朝相反的方向旋转，左边的以更低的速度旋转
200 向左转圈 两个电机朝相同的方向旋转，但速度相同
[0 - -100] 右转 两个电机朝相同的方向旋转，右边的以更低的速度
-100 绕右轮右转 只有左边的电机移动
[-100 - -200] 向右转弯 两个电机朝相反的方向旋转，右边的以更低的速度旋转
-200 向右转圈 两个电机朝相同的方向旋转，但速度相同

当端口连接不同时，左轮为较低端口，右轮为较高端口时，请反映情况。

欢迎你进一步提升远程控制工程。你可以使用游戏杆而不是你的键盘的方向键。或者你可以使用智能手机的陀螺仪传感器。但这是你的项目，我们将远程控制留在了后面（至少目前为止）。

具有两个驱动轮的车辆的定义明确且可预测的运动

我们仍然专注于具有两个驱动轮的车辆及其运动的准确性。远程控制是一种非常特殊的情况，如果需要，人类的头脑会监督车辆的运动并立即进行一些修正。这种情况不需要像转弯半径那样的单位。这就像驾驶汽车一样，没有必要确切知道转弯半径是如何从方向盘的位置决定的。修正是相对和直观的。机器人在没有外部控制的情况下移动，它们的算法需要知道参数的确切依赖性。我们将编写控制车辆运动的程序。我们从最难的变体开始，不存在任何校正机制。这意味着，我们需要能够预测和精确描述车辆运动的函数。我想到以下几点：

  • drive_straight(speed:int, distance: float=None)，其中

    • speed 的符号描述了方向（向前或向后）
    • speed 的绝对值描述了速度。我们更喜欢每秒 SI 单位计，但它是百分比。
    • distance 为 None 或正的，它以 SI 单位 meter 给出。如果它是 None，则运动是无限的。

  • drive_turn(speed:int, radius_turn:float, angle:float=None, right_turn:bool=False)，其中

    • speed 的符号描述了方向（向前或向后）
    • speed 的绝对值描述了速度。
    • radius_turn 是转弯的半径，单位为 meter。我们取两个驱动轮的中间作为我们的参考点。
    • radius_turn 的符号描述了转弯的方向。正值表示左转，正转向，负值代表顺时针转动。
    • angle 为 None 或正的，它是以度为单位的圆弧段 (90° 为四分之一圆，720° 为两个完整的圆)。如果它是 None，则表示无限移动。
    • right_turn 是一个用于特殊情景的标记。如果我们打开 place，属性radius_turn 为零并且没有符号。在这种情况下，左转是默认值，属性right_turn 是相反方向的标志。

正如你可能想象的那样，我们并没有走太远。我们将使用操作 opOutput_Ready，opOutput_Start 和 opOutput_Speed_Sync。这是说我们将不使用中断。从这个角度来看，我们可以回到第二课的知识。

确定车辆的尺寸

们需要车辆的某些尺寸来将上述功能的 SI 单位转换为参数 turn 和操作opOutput_Speed_Sync 的步骤。 车辆的尺寸为：

  • 驱动轮的半径：radius_wheel。
  • 车辆 tread。

使用尺子量出（对于我的车辆）：

  • radius_wheel = 0.021 m
  • tread = 0.16 m

具有更高精度的替代方案是测量车辆的运动。要获得驱动轮的半径，可以使用以下直接命令：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
----------------------------------------------------------                   
 \ 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 \  \  \  \  \        \  \  \  \  \  
                   ----------------------------------------------------------

拿出尺子并测量你的小车移动的距离（如果你的小车没有直行，寻找轮子的最佳结合，而不是外观，这似乎是相同的尺寸，真的是）。轮子的一个完整旋转的距离计算公式为 2 pi radius_wheel。3,600 度为 10 个完整旋转，因此下面的计算给出了你的轮子的 radius_wheel：

1
radius_wheel = distance / (20 * pi)

这将我的轮子的半径修正为 radius_wheel = 0.02128 m。接下来，我们让车辆旋转，并计数 N，车辆转数。为此，我们发送直接命令：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
----------------------------------------------------------------                   
 \ 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 \  \  \  \        \        \  \  \  \  \  
                   ----------------------------------------------------------------

我计算了我的车辆 N = 15.2 完整旋转。两个轮子都旋转了 18, 000°，这是50 个完整的旋转，或者距离 50 2 pi radius_wheel。旋转的半径为 0.5 tread（我们定义轮子的中间为我们的参考点）。这就是说 N 2 pi 0.5 tread = 50 2 pi * radius_wheel 或者：

1
tread = radius_wheel * 100 / N

这修正了 tread 的尺寸（在我的情况中：tread = 0.1346 m）。稍后我们将执行一些额外的运动，也许这将再次修正 tread 的尺寸。

获取参数 STEP 和 TURN 的数学转换

OK，我们知道了车的尺寸。接下来我们需要把我们的方法 drive_straight 和 drive_turn 的参数转为操作 opOutput_Step_Sync 的参数 STEP 和 TURN。这需要一些数学的东西。如果你对细节不感兴趣，你可以只看本小节最后的结果。但是大多数人都想知道细节，细节在这里。

给定我们的小车需要旋转的 angle 和 radius_turn。在转弯中，两个轮子移动不同的距离。外面的轮子的距离为：2 pi radius_wheel STEP / 360。可以根据转弯的几何形状和车辆踏板的知识计算相同的距离：2 pi (radius_turn + 0.5 tread) * angle / 360。这两个相同距离的描述给出了以下等式：

1
STEP = angle * (radius_turn + 0.5 * tread) / radius_wheel

OK，第一个参数计算出来了，但是我们仍然需要计算 TURN。关键的方法来自转弯的几何学，是说，两个车轮之间的速度比 speed_right / speed_left 与外面和里面的车轮的距离比相同：

1
speed_right / speed_left = (radius_turn + 0.5 * tread) / (radius_turn - 0.5 * tread)

你可能还记得，我们已经知道了两个轮子的速度：speed_right = SPEED 和 speed_left = SPEED * (1 - TURN / 100)。这给出等式：

1
1 / (1 - TURN / 100) = (radius_turn + 0.5 * tread) / (radius_turn - 0.5 * tread)

转换这个等式得到：

1
TURN  = 100 * (1 - (radius_turn - 0.5 * tread) / (radius_turn + 0.5 * tread))

就是这样，果给出转弯运动的尺寸 (angle, radius_turn) 和车辆的尺寸 (tread, radius_wheel)：

  1. 变量 STEP 的计算方法为：

    1
    STEP = angle * (radius_turn + 0.5 * tread) / radius_wheel

  2. 变量 TURN 的计算方法为：

    1
    TURN = 100 * (1 - (radius_turn - 0.5 * tread) / (radius_turn + 0.5 * tread))

在 Python 中，这可以写作：

1
2
3
4
5
6
7
8
9
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

数学转换的控制

我们做一些合理性检查：

  • 以半径 radius_turn = 0.5 * tread 旋转得到 TURN = 100 或 TURN = -100，这是对的。
  • 以半径 radius_turn = 0 旋转得到 TURN = 200 或 TURN = -200，这也是OK 的。

现在，我们回到真正的测试。以 angle = 90° 和 radius_turn = 0.5 m 旋转。在我的情况中，由上面的计算给出 STEP = 2358 和 TURN = 23，这将得到命令：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
----------------------------------------------------------                   
 \ 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 \  \  \  \  \        \  \  \  \  \  
                   ----------------------------------------------------------

确实，我的小车几乎以半径 0.5 m 移动了完美的四分之圆。

增强类 TwoWheelVehicle

数学已经足够了，至少在目前，让我们的编码吧！作为我们的第一个任务，我们修改类 TwoWheelVehicle 的构造函数。我们添加两个尺寸 radius_wheel 和 tread，它们被确定为必要的：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
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

接下来我们编写方法 _drive，它与方法 move 非常接近。调用者传入参数 speed，turn 和 step 调用它。外部的世界用 radius_turn 和 angle 来思考，但它们必须被转为内部的参数 turn 和 step。这是说，方法 drive_straight 和 drive_turn 执行转换，然后它们调用内部的方法 _drive：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
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)

我们区分 SYNC 和 ASYNC 或 STD。在 ASYNC 或 STD 的情况中，我们在移动开始前等待，在 SYNC 的情况中直到它结束。如果你从 ev3-python3 下载了模块 ev3_vehicle.py，你将找不到方法 _drive。这是一个提示，我们将回到类 TwoWheelVehicle 实现中断。我们编写方法 drive_turn 的代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
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)

非常大的 radius_turn 值将导致一些问题。舍入到整数之后，它们导致直行。在这种情况下我们抛出一个错误。

方法 drive_straight：

1
2
3
4
5
6
7
8
9
10
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)

放松，我们已经实现了一种可以预测车辆的工具。 请做一些测试！

了解车辆的位置和方向

抱歉，之前的一些内容我写了 足够的数学知识，但现在我来到了三角学。但我们处于一种真的值得努力的情境。想象一下，你驾驶你的车并经过了一系列的移动，你需要知道它的位置和方向。我们无需使用传感器就可以做到。相反，我们使用纯数学而不是魔法。

让我们做一些假设：

  • 当你创建类 TwoWheelVehicle 时你的车放置的位置将是你的坐标系统的原点 (0, 0)。
  • 此时指向正前方的方向是 x 轴的方向。
  • y 轴指向你的车的左手边。
  • 你的车的位置由 x 和 y 坐标描述。以米为单位。
  • 你的车的 orientation 是车的原始方向和它的实际方向的差值。以度数为单位。左转增加 orientation，右转减小它。

这次，我不会介绍数学。把它当作原样，或者把它作为一个谜语，你必须解决。但是请把如下的逻辑添加到你的类 TwoWheelVehicle 中：

  • 构造函数：

    1
    2
    3
    self._orientation = 0.0
    self._pos_x = 0.0
    self._pos_y = 0.0

  • drive_straight(speed, distance)

    1
    2
    3
    4
    5
    6
    7
    8
    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)

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    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

完成类 TwoWheelVehicle

我们通过添加更多的功能来完成类 TwoWheelVehicle：

  • 我们添加一个方法 rotate_to(speed: int, o: float)，它做如下的事情：

    • 计算实际的和新的方向的距离
    • 以 radius_turn = 0 调用 drive_turn 旋转小车，以使其获得新方向。

  • 我们添加一个方法 drive_to(self, speed: int, x: float, y: float)，它做如下的事情：

    • 计算实际的位置和新位置的距离：
      1
      2
      diff_x = pos_x - self._pos_x
      diff_y = pos_y - self._pos_y

我们需要坐标和绝对值：

1
distance = math.sqrt(diff_x**2 + diff_y**2)

- 计算新位置的方向。这很棘手，你需要彻底了解三角学，因为你必须使用 `atan`，`tan` 的反函数。我给你一个提示：

1
2
3
4
5
6
7
8
9
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
- 调用 `rotate_to`，以使 `orientation` 指向 `direction`。
- 调用 `drive_straight` 移动小车到新的位置。

我们做一些测试：

  • 我们把小车发送到一些循环行程中，并编码一系列 drive_to，最终在 position = (0,0) 结束。我们最后添加一个以 orientation = 0 对 rotate_to 的调用。请评估，小车是否真的回到了它最初的位置和方向。
  • 我们给循环行程添加一些 drive_turn 并评估，drive_turn 的错误是大于还是小于 drive_to 的。

以大 radius_turn 旋转将有一个糟糕的精度。这也是舍入的结果。在这种情况中，将 TURN 舍入为整数值创造了错误。

如果你从 ev3-python3 下载了 ev3_vehicle 模块，当你调用它的方法 drive_straight，drive_turn，rotate_to 或 drive_to 时，你最后需要添加一个对 stop 的方法调用！

异步和同步运动

让我们仔细看看我们的车辆驾驶情况。这是我的循环行程的程序：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#!/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)

这个程序的输出：

1
2
3
4
5
6
7
8
9
10
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|

在五毫秒内这个程序给 EV3 设备发送了所有的直接命令，它们被放入队列并等待执行。这个代码很简单，但是会阻塞 EV3 设备直到开始执行最后的命令。请遵循代码并反映出属性 pos_x，pos_y 和 orientation 的值，以及它们是否与我们车辆的实际位置和方向相对应。

这是异步行为！ 程序和 EV3 设备在不同的时间尺度上运行。

现在我们改为同步模式并比较行为：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#!/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)

此版本产生以下输出：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
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|

车辆的运动是相同的，但现在程序发送一个直接命令并等待它完成，然后它发送下一个。sync_mode = SYNC 如它设计那样工作，它同步程序和EV3 设备的时间尺度。另一个好处是，它控制每个直接命令的成功并直接作出反应，如果出现意外情况。

这两个版本（异步和同步）都会阻塞 EV3 设备。

结论

类 TwoWheelVehicle 通过两个驱动轮控制小车的运动。如果我们知道课程的几何形状，我们就可以编写一个程序，通过它来驱动我们的车辆。

我们已经看到了同步和异步模式之间的区别。目前，我们更倾向于 SYNC。但我们的目标是一个解决方案，它可以同步驱动车辆并且不会阻塞EV3 设备。这将打开多任务的大门。

我的 EV3TwoWheelVehicle 类实际上有以下状态：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
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).

我希望，你感觉到，我们现在正在做实际的事情。 对我来说，这是一个亮点。 只有在极少数情况下，人们才能用这么少的努力获得这么多。

原文

坚持原创技术分享，您的支持将鼓励我继续创作！