# 嵌套任务队列和信号发生器

{% embed url="<https://www.bilibili.com/video/BV1d97nzmEHj/>" %}

## 嵌套任务队列

您可以合并多个串口命令作为一个任务队列：

* 令牌是T\_TASK\_QUEUE（'**q**'）
* 使用'**q**'来开始任务队列
* 添加串口指令做为子命令
* 使用'**:**'来添加延迟时间（**必须**）
* 添加 '**>**' 来结束子命令

例如：***`qksit:1000>m 8 0 8 -30 8 0:500>`*** 将使机器人坐下然后移动肩关节。

## 对每个关节使用三角函数生成角度值，以组成平滑且周期性的运动

### 关节舵机旋转角度计算函数

```cpp
JointAngle[index]= signalGenerator(amplitude, midpoint, freq, phase, resolution, frame)
```

* 令牌是T\_SIGNAL\_GEN（'**o**'）
* 命令格式是：**o** 分辨率, 速度，关节序号1， 中位角度值，幅度， 频率，相位，关节序号2， 中位角度值， 幅度， 频率，相位，......\
  例如：o 1 0, 0 40 -20 4 0, 1 -30 20 4 30, 8 -70 10 4 60, 12 -10 10 4 0, 15 10 0 4 0
* 分辨率决定了它是如何增加到 360 ：t += 分辨率
* 速度定义了过渡速度。它将朝着目标角度移动\[1\~125]度。0 表示最大速度
* 频率定义了关节在一个循环中可以振荡的周期数。
* 相位取值范围是-120 到 120。比如相位=30 表示平移 Pi/2。因为120 是一个完整周期（2Pi）。
* 使用逗号或空格来分隔数字，使用逗号分组可以更清晰地表示一个关节的参数（frame）

### 代码实现计算公式

```cpp
for (int t = 0; t < 360; t += resolution)  
  angle = midpoint + 
         round(amp * sin(2.0 * M_PI * ((t + phase * 3 / freq) / (360.0 / freq))));
```

* 运动的迭代器从 0 循环到 360
* 例如，头的俯仰(对应关节序号1)/偏航角(对应关节序号0)可以绑定形成利萨如图形\
  `qksit:100>o 1 8, 0 0 30 4 0, 1 -30 30 4 30:100>o 1 0, 0 0 30 4 0, 1 -30 30 4 15:100>o 1 0, 0 0 30 4 0, 1 -30 30 8 30:100>o 1 8, 0 0 30 8 0, 1 -30 30 4 30:100>o 1 8, 0 0 30 4 0, 1 -30 30 16 30:100>o 1 0, 0 0 30 32 0, 1 -30 30 8 0:100>Wash face:qksit:100>i0 20 1 0 8 -70 12 0 15 10:0>o1 0, 0 40 -20 4 0, 1 -30 20 4 30, 8 -70 10 4 60, 12 -10 10 4 0, 15 10 0 4 0:100>m0 0 1 -20 2 0:0>ksit:0`

{% hint style="info" %}
详细实现，请参考 OpenCatEsp32/src/OpenCat.h 和 reaction.h 中的源代码。
{% endhint %}