基于运动合成分解的舵轮底盘运动模型（以正三角形三轮底盘为例）

目录

1.总述

2.车轮参考方向

3.坐标系

4.停止

5.仅平移

6.仅旋转

7.平移旋转

8.电机驱动

9.原方案（弃用）

正三角形

正方形

等腰三角形

等腰三角形（重制）

附录

现代码

原代码

头文件

此文档原本是对附录中代码的解释，也可单独作为舵轮底盘分析的参考，除第9节的弃用方案外都已经过实践验证，第9节内容及原代码虽最终未被采用但其思路方法或许仍有些许值得参考借鉴之处，故也保留未删，也欢迎批判指正。各小节排列顺序即程序中顺序，代码已有较详细的注释，不再逐一说明功能。

以下所有出现的变量除有说明外，都是结构体Wheel3（三轮底盘）中的变量，所有角度变量若无说明都为弧度制。由于调试时有过多次修改，部分代码可能和说明不一致。

程序代码可分为三部分，第一部分为世界坐标系转机器人坐标系，单独作为一个函数；第二部分是另一个函数的上半部分，运动模型分析；剩余是第三部分，利用运动模型输出驱动电机。第二三部分间以分割线隔开。 

绘图所使用软件为GeoGebra。

1.总述

车体在平面上的运动有三个自由度，分别为二维平面上位置的两个自由度和自身旋转的一个自由度，可以用三个变量表示，分别为

与车头方向垂直的速度（设向右为正方向）：V_x

 与车头方向同向的速度（设向前为正方向）：V_y

使车体自旋的角速度（程序中为V_z）：\omega

这三个变量是运动模型的仅有的三个输入量，构成车体运动状态的三维向量（以下称作运动状态），需要根据它计算出每个轮应转到的角度\theta(程序中未)和运动速度V，即在二维平面上的速度向量（以下称作速度向量），每个轮都有一个这样的速度向量。此运动模型所实现的功能可以用如下变化表示，由于底盘上有三个舵轮，右侧实际是一个2*3的矩阵：

为了后续处理的便利，在函数内定义平移合速度V,\left|V\right|=\sqrt{​{V_x}^2+{V_y}^2}=\omega R，其中R为旋转半径，R=\frac{V}{\omega}。

直接求解车体在某一运动状态下各舵轮的速度向量较为复杂，将运动分解后分析以简化过程。车体的移动速度可以分为在二维平面xy上平移的速度和自身旋转速度两部分，在某一运动状态下，分别计算车体平移和旋转时每个轮子的速度向量，并将这两个速度合成即可计算出这一运动状态各轮所需的速度向量。此方法可以在运动状态的三个变量取包括0在内的任意值时使用，不需分类讨论。

以下为曾用方案（理论可行，见第9部分及附录中原代码，实际未实现预期效果）

可以把所有运动都看成绕车体平面任意一点的圆周运动，直线运动和自旋只是他的特殊极限运动状态；车体包括轮子在内的所有组成部分围绕某一点做圆周运动时角速度\omega相同，线速度和圆心垂直。所以所有轮子和车体自旋的角速度都为\omega。  

注意到当角速度\omega为零时半径R为无穷，无法正常计算，故单独编写。此时为简单平移，所以轮的运动速度都为V，若V_x不为零则转过角度在世界坐标系中角度为arccos[\frac{V_x}{V}sgn(V_y)],若V_x为零则角度根据V_y为向前或后对应角度。

2.车轮参考方向

定义当电机正向旋转时，使底盘能顺时针旋转的方向为模型中各舵轮的参考方向，也是各舵轮起始时应在的位置，在此位置编码器的读数为0，如图2-1所示。A、B、C三点代表三个舵轮，A、B、C轮编号依次为1、2、3。

图2-1 正三角形模型参考方向

3.坐标系

在运动模型分析时都以机器人坐标系为参考坐标系，其xy方向即速度的xy方向，但也可以通过坐标系转化的方法使运动的坐标系为世界坐标系。具体方法为给出特定的世界坐标系下的速度，利用坐标系转换公式将此速度化为机器人坐标系下的速度，在此过程中应当注意符号的正负问题，尤其是旋转正方向，在调试时曾因设定的旋转正方向和传感器的正方向相反导致车身旋转时坐标系与车身旋转方向同向加倍旋转的情况，之后修改了转换公式，使x轴镜像改变了旋转正方向，解决了此问题。

图3-1 角度方向

采用的坐标系是常用的右手螺旋直角坐标系（由于只在平面上运动，不涉及z轴，z代表旋转），角度从x正方向开始随着逆时针旋转逐渐增大，反之为负，如图3-1中α角度是正值，β角度是负值。

 图3-2 机器人坐标系和世界坐标系

如图3-2所示，设图中坐标轴为世界坐标系，机器人抽象为A点，向量u为x轴正方向，v为y轴正方向。当机器人运动时，u、v也会跟随机器人一起运动，而世界坐标系不受影响，根据需求的不同可以选择不同坐标系，如要使机器人移动到场地中某一位置时使用世界坐标系会比机器人坐标系更合适，要机器人进行前进后退等运动时则使用机器人坐标系更合适。

图3-3世界坐标系速度转机器人坐标系速度

如图3-3，由于模型中所有平移运动都是基于机器人坐标系实现的，假如要使机器人以世界坐标系中速度w运动，需要先将此速度转化为机器人坐标系中的速度a、b，之后根据a、b进行计算。旋转速度在两个坐标系中是一致的，不需要转化，可以直接赋值。

4.停止

设计有两种停止情况：

  1. 没有收到运动指令（运动状态三个值都为0，无操作时自动进入），即模型部分程序中最后一个else，不进行任何动作，所有车轮方向保持不变，速度都设置为0。当运动状态有非0值时可以立刻继续运动。
  2. 驻停。通过设置halt的值，当值非0时执行驻停。所有轮都将转至与车体旋转时轮方向的垂直方向，设置速度为0，此时车身不易因外界原因移动，也不会响应运动指令。此状态可以防止不经意误触导致车运动，也可以防止在机器人发射球时防止车身因后坐力旋转移动，但进入此状态时会导致车身偏转一定角度，导致瞄准偏差，尝试过调整车轮旋转方向和旋转速度，未能有效解决此问题，最终未在射球时使用。

5.仅平移

此时车体自旋的角速度\omega为0，仅有与车体方向同方向的速度V_x与车体方向垂直的速度V_y（其中可有一个为0）。可以计算出其合速度V，此时旋转半径R=V\omega无穷大。使车往指定方向运动只需将所有车轮朝向同一方向并转动。

使用平方和可以计算合速度大小，利用反三角函数可以计算出V_x和V_y不同比例时，在车坐标系下对应的角度。程序中使用如下公式：

theta=acos(\frac{V_x}{V})

\left|V\right|=\sqrt{​{V_x}^2+{V_y}^2}

图5-1中u、v代表V_x和V_y，w即合速度V，通过上述公式即可获得需要的值。程序中角度返回值为弧度制，经加减2π限幅后在[-π, π]范围内，以便后续驱动电机转动时的计算。

图5-1 旋转角度及移动速度

特殊地，因V_x=0时返回值为0，无法区分V_y的正负方向，单独对此时V_y正负的情况进行定义，V_y为正时角度π/2，V_y为负时角度-π/2。

6.仅旋转

此时车体自旋的角速度\omega不为0，平移合速度V为0，车绕自身中心旋转，旋转半径R=\frac{V}{\omega}=0。

此时所有轮都将转动到能使车体转动的特定角度（具体角度根据底盘形状而定，速度向量垂线过几何中心，一车如图1-1，即各车轮参考方向），各轮速度都为车身尺寸参数L乘车体自旋的角速度\omega，车身尺寸参数L是各轮到旋转中心的距离，单位自定，用于使不同底盘在同样旋转速度下效果一致。

7.平移旋转

图7-1 机器人坐标平移旋转 

 图7-2 世界坐标平移旋转

使用平移和旋转速度正交分解后叠加即可同时平移旋转，在使用机器人坐标系时，每一时刻机器人都是在当前角度基础上进行旋转，而平移方向则受到了之前已转过角度的影响，最终表现为转弯；使用世界坐标系时因可以感知到已转过的角度，维持平移方向为世界坐标系中的方向，表现为平移同时在平移路径上旋转，运动是平移旋转的简单叠加。两种不同坐标系下的运动的区别如图7-1和图7-2所示，二者都是在向右平移的同时逆时针旋转，机器人坐标系下轨迹为弧线，世界坐标系下轨迹为直线。

图7-3 平移旋转速度

同时存在平移和旋转速度时各轮对应的速度向量如图7-3，向量a是车体平移速度，向量b是车体旋转速度，u、v、w是各轮平移时的速度，u’、v’、w’是各轮旋转时的速度，计算二者的向量和即可得到各轮旋转的速度和方向，如图7-3，但为便于计算在程序中使用正交分解方式进行计算。

图7-4 速度向量合成

特殊地，以此方式实现平移旋转的代码同时也能实现仅平移或仅旋转功能。（原方法需分类讨论，其适用性窄且代码较复杂难以维护，已弃用）

图7-5 正交分解

现有程序中采用的方案是先计算平移速度，如图7-5，将平移速度分解为x、y方向上分速度Vx_part和Vy_part，底盘形状和每个轮的位置等几何关系决定了旋转速度的方向，将旋转速度分别分解后叠加到每个轮分速度Vx_part和Vy_part上，对于某个特定的轮，由于旋转速度的角度是固定的，x、y方向上所叠加的值是旋转速度大小乘一个常数。如对于一车1号轮，旋转速度方向正好和x方向重合，x方向系数1，y方向系数0；2号轮旋转速度方向在坐标系中为120度，x方向系数\cos{\left(\frac{2}{3}\pi\right)}=-\frac{1}{2}，y方向系数sin(\frac{2}{3}\pi)=\frac{\sqrt{3}}{2}。

计算完分速度Vx_part和Vy_part后可以通过平方和计算出合速度大小，将Vx_part和Vy_part看作V_x和V_y，以计算平移时轮速度向量的方法分别计算每个轮的速度向量，区别在于仅平移时所有轮的旋转方向是一致的，而此时每个轮的旋转方向可能是不一致的。

8.电机驱动

编码器数值=外部齿轮比*电机编码器齿轮比*电机转动一圈数值变化*转动弧度/（2*PI）

通过以上计算得出了各轮分别需要的朝向和转动速度，之后需要利用这些数值使电机以特定方式转动以满足需求。舵轮使用小电机完成舵轮自身朝向旋转，程序中通过改变C610结构体下电机其他位置使电机带动舵轮转向指定角度；大电机通过设置期望速度使轮胎转动，进而使机器人运动。这两种电机都配有编码器，可以实时监测电机旋转位置或速度。

图8-1 初版防绕线

图8-2 大幅度旋转

最初由于机械结构和走线的限制，舵轮只能旋转特定角度，继续旋转就可能会使线相互缠绕过度以至松开、断裂，故需限制允许旋转的角度。最初一版中旋转角度限制为正负90度，即图8-1中第一四象限区域，其余二三象限以正负90度范围内大电机反转实现同样效果，例如要转到图8-1中向量u方向则转到到向量v方向同时大电机。但缺点在于在正负90度的边界区域改变方向时，由于采取了反转进行角度映射的方法，舵轮自身会近180度大角度旋转导致运动稳定性差，如图8-2，上一时刻速度向量u，下一时刻速度向量w，则轮实际朝向会由v到w，尤其是当速度方向频繁在边界两侧来回变化时会对正常行驶造成巨大影响，显然机械结构和电机控制方法都存在很大缺陷，对应代码为原代码中分隔线之下部分。

图8-3 改进旋转方案

后续由于机械结构改进，可以允许至少正负360度范围轮的旋转，也随之改进了电机的运动方法，在360度范围内使小电机旋转角度将尽可能小，正常情况下在90度内。实现此功能同样也利用大电机反转以等效舵轮旋转180度，但在使小电机旋转时会根据内置编码器返回值在转至期望位置或期望位置正负半圈等三个位置中选择所需转动角度最小的一个，转至期望位置正负半圈时大电机反转以等效舵轮旋转180度，理论上选取到的最小转角应小于90度，但为保险起见也考虑了最小转角大于90度的情况，同样使大电机反转以等效舵轮旋转180度，反向旋转所得最小转角的补角。在如图8-3所示情况下，轮当前朝向为u，标示角度范围α为下一时刻可能会直接旋转到的朝向范围，可以旋转角度β到v方向，而要转至范围外的a方向则转至w并反转大电机。但当选取到的转动位置超出360度范围时，仍将和原版本一样强制回转近180度并使大电机反转，对应代码为现代码中分隔线之下部分。

若采用导电滑环等可以近无限圈旋转的机械结构，则可以取消或极大拓展正负360度的角度限制，使舵轮每次旋转角度始终小于90度，避免近180度大幅度旋转的情况。

9.原方案（弃用）

此方法参考了四舵轮运动算法解析_ddydy111的博客-CSDN博客_四舵轮运动模型中的思路，最初使用此方法来处理同时具有平移速度和旋转速度时舵轮运动，并根据需要对正三角形、正方形、直角三角形都进行了分析。

此方案主要由平移旋转时几何关系确定各轮的速度向量，但只能在三个量都不为0时使用，其余情况只能另外分类讨论（仅平移和仅旋转，此部分仍在使用，并与平移旋转融合为一体），相对前面所述方法具有较高的局限性，且运算过程复杂，出现问题时难以快速找到问题并排除，也难以验证结果是否正确，测试时最终仅在部分角度范围实现了预期效果，由于计算过程较为复杂，难以分析在哪一环节出现错误，也没有让使用此模型的底盘下地测试过实际效果，最终没有继续采用此方法，改用了前文中介绍的方式。附录中原代码中分隔线之上的部分即使用此方法的正三角形底盘代码，且由于其大多是数学计算的特殊性质，在编写程序前先分析计算并编写了以下文档，之后根据相关公式逐一转为代码。若有兴趣可以继续阅读，跳过此部分也无妨，也欢迎对其中错误不当之处等批判指正。

正三角形

图9-1-1 正三角形舵轮运动模型示意图

如图9-1-1，将车体视为正三角形，将车头方向（HA）和车体运动方向（HI）之间的夹角设为α（从AH开始逆时针旋转，角度为[-π, π],由trans_PI函数限制）,设三角形中AH长度为L，D点为圆周运动的圆心，设车圆周运动的半径R，连接三个舵轮所在的三顶点和和圆心D，连线长度即各轮的圆周运动半径RA、RB、RC。若得知半径大小和线段相对车体的方向即可计算出舵轮应转动的角度AA、AB、AC和速度VA、VB、VC。

半径

首先求解三条半径长度，如图一，作三条垂直辅助线，由勾股定理根据R可计算出出各轮的运动半径，求解如下：

R_A=\sqrt{\left(L+HG\right)^2+{GD}^2}

R_B=\sqrt{​{(L+HE)}^2+ED^2}

R_C=\sqrt{​{(L+HF)}^2+{FD}^2}

其中HE、HF、HG应区分正负，位于AH、BH、CH延长线上时为正，否则为负。

根据几何关系，（为保证公式在全平面成立经过一些正负、加减π处理）

∠DHE=-π/6+α

∠DHF=π/6+α+π

∠DHG=π/2+α

则可将三条边表示为

R_A=\sqrt{\left(L+Rcos(\frac{\pi}{2}+\alpha)\right)^2+\left(R{sin(\frac{\pi}{2}+\alpha))}^2\right)^2}

R_B=\sqrt{\left(L+Rcos(-\frac{\pi}{6}+\alpha)\right)^2+\left(Rsin(-\frac{\pi}{6}+\alpha)\right)^2}

R_C=\sqrt{\left(L+Rcos(\frac{\pi}{6}+\alpha+\pi)\right)^2+\left(Rsin(\frac{\pi}{6}+\alpha+\pi)\right)^2}

经检验公式在全平面成立。

速度

至此利用各半径可求得舵轮运动速度

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

角度

借助求半径时构建辅助三角形，可以得到

\angle AHG=arctan(\frac{Rsin(\frac{\pi}{2}+\alpha)}{L+Rcos(\frac{\pi}{2}+\alpha)})

\angle BHE=\ arctan(\frac{Rsin(-\frac{\pi}{6}+\alpha)}{L+Rcos(-\frac{\pi}{6}+\alpha)})

\angle CHF=\ arctan(\frac{Rsin(\frac{\pi}{6}+\alpha+\pi)}{L+Rcos(\frac{\pi}{6}+\alpha+\pi)})

将车体绕几何中心旋转时舵轮的方向定为参考角度0度，逆时针旋转，角度从0到2π（最终计算结果范围为[-π, π]），如图9-1-2。

图9-1-2 参考角度示意图

arctan函数最大范围为(-\frac{\pi}{2},\frac{\pi}{2}),需分类讨论：

由几何关系验证，当arctan函数分母为正时，

AA=∠AHG

AB=∠BHE

AC=∠CHF

当arctan函数分母为负时，

AA=∠AHG+π

AB=∠BHE+π

AC=∠CHF+π

此时已获得三个舵轮的速度和方向，可直接控制舵轮的运动。

正方形

图9-2-1 正方形舵轮运动模型示意图

四轮和三轮情况类似，将车体视为正方向，如图9-2-1，将车头方向（EH）和车体运动方向（EG）之间的夹角设为α（从EH开始逆时针旋转，角度为0-2π度）,设正方形中AH长度为L，F点为圆周运动的圆心，设车圆周运动的半径R，连接四个舵轮所在的四顶点和和圆心F，连线长度即各轮的圆周运动半径RA、RB、RC、RD。若得知半径大小和线段相对车体的方向即可计算出舵轮应转动的角度AA、AB、AC、AD和速度VA、VB、VC、VD。

半径

求解四个舵轮分别运动的半径，如图三，作过几何中心垂直于正方形变的辅助线，从F作两条辅助线的垂线,可得

EI=Rsin\alpha

FS=Rcos\alpha

由勾股定理根据R可计算出出各轮的运动半径，求解如下：

R_A=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

R_B=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

R_C=\sqrt{\left(L-Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

R_D=\sqrt{\left(L-Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

经检验公式在全平面成立。

速度

至此利用各半径可求得舵轮运动速度

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

V_D=R_D\omega

角度

将车头的方向定为参考角度0度，逆时针旋转，角度从0到2π度。

借助求半径时构建辅助三角形，可以得到

\angle AFI=arctan\left(\frac{L-Rsin\alpha}{L+Rcos\alpha}\right)

\angle BFI=arctan\left(\frac{L+Rsin\alpha}{L+Rcos\alpha}\right)

\angle CFJ=arctan\left(\frac{L+Rsin\alpha}{L-Rcos\alpha}\right)

\angle DFJ=arctan\left(\frac{L-Rsin\alpha}{L-Rcos\alpha}\right)

arctan函数最大范围为(-\frac{\pi}{2},\frac{\pi}{2}),需分类讨论：

由几何关系验证，当arctan函数分母为正时，

AA=∠AFI

 AB=-∠BFI

AC=∠CFJ

AD=-∠DFJ

当arctan函数分母为负时

AA=π+∠AFI

AB=π-∠BFI

AC=π+∠CFJ

AD=π-∠DFJ

此时已获得四个舵轮的速度和方向，可直接控制舵轮的运动。

等腰三角形

图9-3-1 直角三角形舵轮运动模型示意图

当底盘形状为等腰三角时，与正方形四轮时一致，仅取A、B、C三点对应值即可，如图9-3-1。之前所得结论均适用，为适应形状的改变，对参考方向也进行了一定的调整，如图9-3-2。

图9-3-2 等腰三角形模型参考方向

旋转坐标系，使其车头前方为y轴正方向，右侧为x轴正方向，角度为从x轴正方向开始逆时针旋转（即一般直角坐标系的定义），如图三中黄色直线所示。此坐标系为原坐标系顺时针旋转45度所得，故由原坐标系转至新坐标系时应+45°(+π/4)，此外还要加上因参考角度不同导致不同轮角度额外调整不同角度。

图9-3-3 旋转后坐标系

半径

与正方形时完全一致。

R_A=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

R_B=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

R_C=\sqrt{\left(L-Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

速度

与正方形时完全一致。

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

角度

根据坐标系、参考方向的调整进行修改。

A_A=\pi+\angle AFI+\frac{\pi}{4}-\frac{\pi}{4}=\pi+\angle AFI

A_B=\pi-\angle BFI+\frac{\pi}{4}+\frac{\pi}{4}=\frac{3\pi}{2}-\angle BFI=-\frac{\pi}{2}-\angle BFI

A_C=\pi+\angle CFJ+\frac{\pi}{4}+\frac{3\pi}{4}=2\pi+\angle CFJ=\angle CFJ

等腰三角形（重制）

直接如图9-3-3建立坐标系,重新为等腰三角形建立模型，不再借助原正方形模型。仍计算全部四点的值，使用时仅取A、B、C三点。

车头方向为图8-3中BE射线方向，将车头方向（BE）和车体运动方向（EG）之间的夹角设为α（改变正方形模型车头方向）。定义L为斜边一半（改变定义）。按同样思路求解。

半径

同样由半径点F作两坐标轴垂线，图略。

R_A=\sqrt{\left(L+Rcos\alpha\right)^2+\left(Rsin\alpha\right)^2}

R_B=\sqrt{\left(Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

R_C=\sqrt{\left(L-Rcos\alpha\right)^2+\left(Rsin\alpha\right)^2}

R_D=\sqrt{\left(Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

速度

利用各半径可求得舵轮运动速度

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

V_D=R_D\omega

角度

将车头的方向定为参考角度0度，逆时针旋转。

借助求半径时构建辅助三角形，可以得到

\angle AFI=arctan\left(\frac{Rsin\alpha}{L+Rcos\alpha}\right)

\angle BFI=arctan\left(\frac{L+Rsin\alpha}{Rcos\alpha}\right)

\angle CFJ=arctan\left(\frac{Rsin\alpha}{-(L-Rcos\alpha)}\right)

\angle DFJ=arctan\left(\frac{-(L-Rsin\alpha)}{Rcos\alpha}\right)

arctan函数最大范围为(-\frac{\pi}{2},\frac{\pi}{2}),需分类讨论： 

由几何关系验证，当arctan函数分母为正时

AA=∠AFI

 AB=∠BFI

AC=∠CFJ

AD=∠DFJ

当arctan函数分母为负时

AA=π+∠AFI

AB=π+∠BFI

AC=π+∠CFJ

AD=π+∠DFJ

由于参考角度与此时角度存在差异，继续修正角度

A_A\prime=A_A-0

A_B\prime=A_B-\frac{\pi}{2}

A_C\prime=A_C-\pi

A_D\prime=A_D-\frac{3\pi}{2}

此时已获得四个舵轮的速度和方向，可直接控制舵轮的运动。

附录

现代码

  1. /*
  2. *********************************************************************************************************
  3. * Brief    : World frame to robot frame transform formula.
  4. *
  5. * Param(s) : world - velocity vector(Vx,Vy,Vz) in inertia frame. (mm/s, rad/s)
  6. *      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
  7. *
  8. * Return(s): none.
  9. *
  10. *********************************************************************************************************
  11. */
  12. void Formula_World2Robo (Velocity *world, Velocity *robo)//转角相反
  13. {
  14.   float sinTheta = sin(RobotPos.theta);
  15.   float cosTheta = cos(RobotPos.theta);
  16.   
  17. robo->Vx = world->Vx * cosTheta + world->Vy * sinTheta;
  18. robo->Vy = world->Vy * cosTheta - world->Vx * sinTheta;
  19. robo->Vz = world->Vz;
  20. }
  21.  
  22.  
  23. /*
  24. *********************************************************************************************************
  25. * Brief    : robot frame to wheel frame transform formula.
  26. *
  27. * Param(s) : wheel - velocity vector(Vx,Vy,Vz) in wheel frame. (0.1 enc counts/s)
  28. *      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
  29. *
  30. * Return(s): none.
  31. *·
  32. *********************************************************************************************************
  33. */
  34. //float theta_angle[3];//转向以角度形式展示
  35. void Formula (Velocity *robo)
  36. {
  37.   int MULtiple=1;
  38.   int flag;
  39.   float V;
  40.   float Vx_part[3]={0,0,0},Vy_part[3]={0,0,0},theta_temp[3]={0,0,0};
  41. Wheel3.L = 35;//车轮到三角中心的距离
  42.  
  43.   
  44.   //输入Vx_in,Vy_in
  45.   //输入Wheel3.omega
  46. Wheel3.Vx = robo->Vx;
  47. Wheel3.Vy = robo->Vy;
  48. Wheel3.omega = robo->Vz * PI/180 * Wheel3.L;
  49. V = sqrt(Wheel3.Vx * Wheel3.Vx + Wheel3.Vy * Wheel3.Vy);
  50.   if (Wheel3.halt)//驻停
  51. {
  52. Wheel3.theta[0] = PI/2;
  53. Wheel3.theta[1] = Wheel3.theta[0];
  54. Wheel3.theta[2] = Wheel3.theta[0];
  55.     
  56. Wheel3.V[0] = 0;
  57. Wheel3.V[1] = Wheel3.V[0];
  58. Wheel3.V[2] = Wheel3.V[0];
  59. }
  60.   else if (V || Wheel3.omega)
  61. {
  62.       if (Wheel3.Vy >= 0)
  63. flag = 1;
  64.       else 
  65. flag = -1;
  66.  
  67.         //反三角计算角度
  68.         if (Wheel3.Vx != 0)
  69. {
  70. theta_temp[0] = flag*acos(Wheel3.Vx / V);
  71. theta_temp[1] = theta_temp[0];
  72. theta_temp[2] = theta_temp[0];
  73. }
  74.         else
  75. {
  76.           if (flag==1)
  77. {
  78. theta_temp[0] = PI/2;
  79. theta_temp[1] = theta_temp[0];
  80. theta_temp[2] = theta_temp[0];
  81. }
  82.           else
  83. {
  84. theta_temp[0] = -PI/2;
  85. theta_temp[1] = theta_temp[0];
  86. theta_temp[2] = theta_temp[0];
  87. }
  88. }//反三角计算角度
  89.         
  90.         //平移旋转速度正交分解叠加
  91. Vx_part[0] = V * cos(theta_temp[0]);
  92. Vx_part[1] = V * cos(theta_temp[1]);
  93. Vx_part[2] = V * cos(theta_temp[2]);
  94. Vy_part[0] = V * sin(theta_temp[0]);
  95. Vy_part[1] = V * sin(theta_temp[1]);
  96. Vy_part[2] = V * sin(theta_temp[2]);
  97. Vx_part[0] += Wheel3.omega;
  98. Vx_part[1] -= Wheel3.omega / 2;
  99. Vy_part[1] += Wheel3.omega * sqrt(3)/2;
  100. Vx_part[2] -= Wheel3.omega /2;
  101. Vy_part[2] -= Wheel3.omega * sqrt(3)/ 2;
  102. Wheel3.V[0] = sqrt(Vx_part[0]*Vx_part[0] + Vy_part[0]*Vy_part[0]);
  103. Wheel3.V[1] = sqrt(Vx_part[1]*Vx_part[1] + Vy_part[1]*Vy_part[1]);
  104. Wheel3.V[2] = sqrt(Vx_part[2]*Vx_part[2] + Vy_part[2]*Vy_part[2]);
  105.         
  106.         //叠加后重新计算角度
  107.         for (int i=0;i<3;i++)
  108. {
  109.           if (Vx_part[i] != 0)
  110. {
  111.             if (Vy_part[i] >= 0)
  112. {
  113. Wheel3.theta[i] =  acos(Vx_part[i] / Wheel3.V[i]);
  114. }
  115.             else
  116. {
  117. Wheel3.theta[i] = -acos(Vx_part[i] / Wheel3.V[i]);
  118. }
  119. }
  120.           else
  121. {
  122.             if (Vy_part[i] == 0)
  123. {
  124. Wheel3.theta[i] = 0;
  125. }
  126.             if (Vy_part[i] > 0)
  127. {
  128. Wheel3.theta[i] = PI/2;
  129. }
  130.             else
  131. {
  132. Wheel3.theta[i] = -PI/2;
  133. }
  134. }
  135. }//叠加后重新计算角度
  136.         //基准位置修正，编码器向后到编码器向里
  137. Wheel3.theta[1] -= 2*PI/3;
  138. Wheel3.theta[2] += 2*PI/3;
  139. }//平移旋转
  140.     else//其他情况:仅停止，不转轮
  141. {
  142. Wheel3.V[0] = 0;
  143. Wheel3.V[1] = Wheel3.V[0];
  144. Wheel3.V[2] = Wheel3.V[0];
  145. }
  146.   
  147.   //限制角度范围
  148. Wheel3.theta[0] = trans_PI(trans_PI(Wheel3.theta[0]));
  149. Wheel3.theta[1] = trans_PI(trans_PI(Wheel3.theta[1]));
  150. Wheel3.theta[2] = trans_PI(trans_PI(Wheel3.theta[2]));
  151. //  //弧度转角度，仅供参考，无实际作用
  152. //  theta_angle[0] = trans_PI(Wheel3.theta[0])*180/PI;
  153. //  theta_angle[1] = trans_PI(Wheel3.theta[1]+PI*2/3)*180/PI;
  154. //  theta_angle[2] = trans_PI(Wheel3.theta[2]-PI*2/3)*180/PI;
  155. //*************************************限制舵轮旋转圈数****************************************************
  156.   //拓展至正负360度
  157.   double Exparg_temp[3][3],diff_arg[3][3];
  158.   int diff_min[3];
  159.   for (int i=0;i<3;i++)
  160. {
  161.     if( -1179504.0 < MOTOR_C610[i+1].Temparg < 1179504.0 )  //(2*PI) 允许自由旋转范围
  162. {
  163. Exparg_temp[i][0] = (int)(Wheel3.theta[i]*187723.89199);//轮朝向转为电机位置
  164. Exparg_temp[i][1] = Exparg_temp[i][0] + 589752.0;//PI 正向多半圈
  165. Exparg_temp[i][2] = Exparg_temp[i][0] - 589752.0;//PI 负向多半圈
  166. diff_arg[i][0] = Exparg_temp[i][0] - MOTOR_C610[i+1].Temparg;
  167. diff_arg[i][1] = Exparg_temp[i][1] - MOTOR_C610[i+1].Temparg;
  168. diff_arg[i][2] = Exparg_temp[i][2] - MOTOR_C610[i+1].Temparg;
  169. diff_min[i] = (abs(diff_arg[i][0]) < abs(diff_arg[i][1]) + 10.0) ? 0:1;
  170. diff_min[i] = (abs(diff_arg[i][2] - 10.0 ) < diff_arg[i][diff_min[i]]) ? 2:diff_min[i];
  171.         if( -294876  < diff_arg[i][diff_min[i]] < 294876 )//当前位置正负90度内（应已包含所有角度）取最小转动角度
  172. {
  173.           if (diff_min[i] == 0)
  174. {
  175. MOTOR[i+1].ExpSpeed = -(int)((Wheel3.V[i])*MULtiple);
  176. MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][0]);
  177. }
  178.           else if (diff_min[i] == 1)
  179. {
  180. MOTOR[i+1].ExpSpeed = (int)((Wheel3.V[i])*MULtiple);
  181. MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][1]);
  182. }
  183.           else if (diff_min[i] == 2)
  184. {
  185. MOTOR[i+1].ExpSpeed = (int)((Wheel3.V[i])*MULtiple);
  186. MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][2]);
  187. }
  188.           else 
  189. {}//错误
  190. }        
  191.         else if( -294876 > diff_arg[i][diff_min[i]] )//负向超过90度（可能始终不会执行）
  192. {
  193. MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
  194. MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][diff_min[i]] + 589752.0); 
  195. }
  196.         else//正向超过90度（可能始终不会执行）
  197. {
  198. MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
  199. MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][diff_min[i]] - 589752.0);
  200. }
  201. }    
  202.     else if( -1179504.0 > MOTOR_C610[i+1].Temparg )//负向超过限幅
  203. {
  204. MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
  205. MOTOR_C610[i+1].Exparg=(int)(-(Wheel3.theta[i]+PI)*187723.89199);//1/(2*PI)*8191*36*4
  206. }
  207.     else//正向超过限幅
  208. {
  209. MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
  210. MOTOR_C610[i+1].Exparg=(int)(-(Wheel3.theta[i]-PI)*187723.89199);//1/(2*PI)*8191*36*4
  211. }
  212. }//for
  213. }//Formula_4Omni末尾
  214.  
  215. float trans_PI(float n)//[-2PI,2PI]->[-PI,PI]
  216. {
  217.   if (n > PI) n -= 2*PI;
  218.   else if (n < -PI) n += 2*PI;
  219.   return n;
  220. }

原代码

  1. /*
  2. *********************************************************************************************************
  3. * Brief    : World frame to robot frame transform formula.
  4. *
  5. * Param(s) : world - velocity vector(Vx,Vy,Vz) in inertia frame. (mm/s, rad/s)
  6. *      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
  7. *
  8. * Return(s): none.
  9. *
  10. *********************************************************************************************************
  11. */
  12. void Formula_World2Robo (Velocity *world, Velocity *robo)
  13. {
  14.   float sinTheta = sin(RobotPos.theta);
  15.   float cosTheta = cos(RobotPos.theta);
  16.   
  17. robo->Vx = world->Vx * cosTheta + world->Vy * sinTheta;
  18. robo->Vy = world->Vx * ( -sinTheta ) + world->Vy * cosTheta;
  19. robo->Vz = world->Vz;
  20. }
  21.  
  22.  
  23. /*
  24. *********************************************************************************************************
  25. * Brief    : robot frame to wheel frame transform formula.
  26. *
  27. * Param(s) : wheel - velocity vector(Vx,Vy,Vz) in wheel frame. (0.1 enc counts/s)
  28. *      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
  29. *
  30. * Return(s): none.
  31. *
  32. *********************************************************************************************************
  33. */
  34.  
  35. void Formula (Velocity *robo)
  36. {
  37.   int MULtiple=1;
  38.   int flag,halt;
  39.   float V;
  40.   float angleA,angleB,angleC,angle[3];
  41.   float cosA,sinA,cosB,sinB,cosC,sinC;
  42.   float ang_action;
  43. Wheel3.L = 35;//车轮到三角中心的距离
  44. ang_action = 0.261799387791;//15 / 180 * PI，action和车头的偏差角15度
  45.   
  46.   //输入Vx_in,Vy_in
  47.   //输入Wheel3.omega
  48. Wheel3.Vx = robo->Vx;
  49. Wheel3.Vy = robo->Vy;
  50. Wheel3.omega = robo->Vz;
  51. V = sqrt(Wheel3.Vx * Wheel3.Vx + Wheel3.Vy * Wheel3.Vy);
  52. halt = 0; 
  53.   if (halt)//驻停
  54. {
  55. Wheel3.theta[0] = PI/2;
  56. Wheel3.theta[1] = Wheel3.theta[0];
  57. Wheel3.theta[2] = Wheel3.theta[0];
  58.     
  59. Wheel3.V[0] = 0;
  60. Wheel3.V[1] = Wheel3.V[0];
  61. Wheel3.V[2] = Wheel3.V[0];
  62. }
  63.   else
  64. {
  65.     if(V)//平移
  66. {
  67.       if (Wheel3.Vy >= 0)
  68. flag = 1;
  69.       else 
  70. flag = -1;
  71.       
  72.       if( Wheel3.omega > 0 )//平移并旋转
  73. {
  74.         //基准位置
  75. Wheel3.alfa = PI/2 + flag*acos(Wheel3.Vx/V) + ang_action;
  76. Wheel3.R = V / Wheel3.omega;
  77. angleA = PI/2 + Wheel3.alfa;
  78. angleB = -PI/6 + Wheel3.alfa;
  79. angleC = PI/6 + Wheel3.alfa + PI;
  80. angleA = trans_PI(angleA);
  81. angleB = trans_PI(angleB);
  82. angleC = trans_PI(angleC);
  83.         
  84.         //中间量
  85. cosA = Wheel3.L + Wheel3.R * cos(angleA);
  86. cosB = Wheel3.L + Wheel3.R * cos(angleB);
  87. cosC = Wheel3.L + Wheel3.R * cos(angleC);
  88. sinA = Wheel3.R * sin(angleA);
  89. sinB = Wheel3.R * sin(angleB);
  90. sinC = Wheel3.R * sin(angleC);
  91.         
  92.         //轮半径
  93. Wheel3.r[0] = sqrt(cosA * cosA + sinA * sinA);
  94. Wheel3.r[1] = sqrt(cosB * cosB + sinB * sinB);
  95. Wheel3.r[2] = sqrt(cosC * cosC + sinC * sinC);
  96.         
  97.         //轮速度
  98. Wheel3.V[0] = Wheel3.omega * Wheel3.r[0];
  99. Wheel3.V[1] = Wheel3.omega * Wheel3.r[1];
  100. Wheel3.V[2] = Wheel3.omega * Wheel3.r[2];
  101.         
  102.         //轮角度
  103. angle[0] = atan(sinA / cosA);
  104. angle[1] = atan(sinB / cosB);
  105. angle[2] = atan(sinC / cosC);
  106. Wheel3.theta[0] = (cosA > 0)?(angle[0]):(angle[0] + PI);
  107. Wheel3.theta[1] = (cosA > 0)?(angle[1]):(angle[1] + PI);
  108. Wheel3.theta[2] = (cosA > 0)?(angle[2]):(angle[2] + PI);
  109. }
  110.       else//不旋转
  111. {
  112.         //统一速度赋值
  113. Wheel3.V[0] = V;
  114. Wheel3.V[1] = Wheel3.V[0];
  115. Wheel3.V[2] = Wheel3.V[0];
  116.         
  117.         //反三角计算角度
  118.         if (Wheel3.Vx != 0)
  119. {
  120. Wheel3.theta[0] = flag*acos(Wheel3.Vx / V);
  121. Wheel3.theta[1] = Wheel3.theta[0];
  122. Wheel3.theta[2] = Wheel3.theta[0];
  123. }
  124.         else
  125. {
  126.           if (flag==1)
  127. {
  128. Wheel3.theta[0] = PI/2;
  129. Wheel3.theta[1] = Wheel3.theta[0];
  130. Wheel3.theta[2] = Wheel3.theta[0];
  131. }
  132.           else
  133. {
  134. Wheel3.theta[0] = -PI/2;
  135. Wheel3.theta[1] = Wheel3.theta[0];
  136. Wheel3.theta[2] = Wheel3.theta[0];
  137. }
  138. }
  139.         //基准位置修正
  140. Wheel3.theta[1] += 2*PI/3;
  141. Wheel3.theta[2] -= 2*PI/3;
  142.  
  143. }
  144. }
  145.     else if (Wheel3.omega != 0)//仅旋转无位移
  146. {
  147. Wheel3.theta[0] = 0;
  148. Wheel3.theta[1] = Wheel3.theta[0];
  149. Wheel3.theta[2] = Wheel3.theta[0];
  150. Wheel3.V[0] = Wheel3.L * Wheel3.omega;
  151. Wheel3.V[1] = Wheel3.V[0];
  152. Wheel3.V[2] = Wheel3.V[0];
  153. }
  154.     else//其他情况:仅停止，不转轮
  155. {
  156. Wheel3.V[0] = 0;
  157. Wheel3.V[1] = Wheel3.V[0];
  158. Wheel3.V[2] = Wheel3.V[0];
  159. }
  160. }
  161.   //限制角度范围
  162. Wheel3.theta[0] = trans_PI(Wheel3.theta[0]);
  163. Wheel3.theta[1] = trans_PI(Wheel3.theta[1]);
  164. Wheel3.theta[2] = trans_PI(Wheel3.theta[2]);
  165.   
  166. //*************************************限制舵轮旋转圈数****************************************************
  167.   
  168.     if( (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199 >= -PI/2) && (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199 <= PI/2) )
  169. {
  170. MOTOR_C610[1].Exparg=(int)(-Wheel3.theta[0]*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
  171. MOTOR[1].ExpSpeed=(int)(Wheel3.V[0]*MULtiple);
  172. }
  173.     else if( (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199) < -PI/2 )
  174. {
  175. MOTOR_C610[1].Exparg=(int)(-(Wheel3.theta[0]+PI)*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
  176. MOTOR[1].ExpSpeed=-(int)((Wheel3.V[0])*MULtiple);
  177. }
  178.     else
  179. {
  180. MOTOR_C610[1].Exparg=(int)(-(Wheel3.theta[0]-PI)*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
  181. MOTOR[1].ExpSpeed=-(int)((Wheel3.V[0])*MULtiple);
  182. }
  183.     if( (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199 >= -PI/2) && (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199 <= PI/2) )
  184. {
  185. MOTOR_C610[2].Exparg=(int)(-Wheel3.theta[1]*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
  186. MOTOR[2].ExpSpeed=(int)(Wheel3.V[1]*MULtiple);
  187. }
  188.     else if( (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199) < -PI/2 )
  189. {
  190. MOTOR_C610[2].Exparg=(int)(-(Wheel3.theta[1]+PI)*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
  191. MOTOR[2].ExpSpeed=-(int)((Wheel3.V[1])*MULtiple);
  192. }
  193.     else
  194. {
  195. MOTOR_C610[2].Exparg=(int)(-(Wheel3.theta[1]-PI)*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
  196. MOTOR[2].ExpSpeed=-(int)((Wheel3.V[1])*MULtiple);
  197. }
  198.     if( (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199 >= -PI/2) && (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199 <= PI/2) )
  199. {
  200. MOTOR_C610[3].Exparg=(int)(-Wheel3.theta[2]*187723.89199)+Encoder_MotorC610[3];///(2*PI)*8191*36*4
  201. MOTOR[3].ExpSpeed=(int)(Wheel3.V[2]*MULtiple);
  202. }
  203.     else if( (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199) < -PI/2 )
  204. {
  205. MOTOR_C610[3].Exparg=(int)(-(Wheel3.theta[2]+PI)*187723.89199)+Encoder_MotorC610[3];//1/(2*PI)*8191*36*4
  206. MOTOR[3].ExpSpeed=-(int)((Wheel3.V[2])*MULtiple);
  207. }
  208.     else
  209. {
  210. MOTOR_C610[3].Exparg=(int)(-(Wheel3.theta[2]-PI)*187723.89199)+Encoder_MotorC610[3];//1/(2*PI)*8191*36*4
  211. MOTOR[3].ExpSpeed=-(int)((Wheel3.V[2])*MULtiple);
  212. } 
  213. }
  214.  
  215. float trans_PI(float n)//[-2PI,2PI]->[-PI,PI]
  216. {
  217.   if (n > PI) n -= 2*PI;
  218.   else if (n < -PI) n += 2*PI;
  219.   return n;
  220. }

头文件

  1. /*
  2. *********************************************************************************************************
  3. *                                             INCLUDE FILES
  4. *********************************************************************************************************
  5. */
  6.  
  7. #pragma once
  8. #include "includes.h"
  9.  
  10. /*
  11. *********************************************************************************************************
  12. *                                                EXTERNS
  13. *********************************************************************************************************
  14. */
  15.  
  16. #ifdef   ROBOT_CTRL_MODULE
  17. #define  ROBOT_CTRL_EXT
  18. #else
  19. #define  ROBOT_CTRL_EXT  extern
  20. #endif
  21.  
  22. //****************************************************************           ???           ****************************************************************
  23. #ifdef   CHASSIS_MODULE
  24. #define  CHASSIS_EXT
  25. #else
  26. #define  CHASSIS_EXT  extern
  27. #endif
  28. //****************************************************************           ???          ****************************************************************
  29.  
  30. /*
  31. *********************************************************************************************************
  32. *                                             	 DEFINES
  33. *********************************************************************************************************
  34. */
  35.  
  36. // CAN configuration
  37. #define SYNC_CYCLE        5  // 100 ms
  38.  
  39. // coefficient needed by transform formula
  40. #define THREE_OMNI_D      386.3 // distance between wheel center and spin center
  41. #define FOUR_OMNI_D       280.0
  42. #define FOUR_Mecunum_L    320.0
  43. #define FOUR_Mecunum_l    355.0
  44.  
  45. // wheel data 
  46. #define WHEELNUM        4
  47. #define	WHEEL_R         100.0 // radius(mm)
  48. #define WHEEL_FORMULA   Formula_4Omni
  49.  
  50. // motor data 
  51. #define GEAR_RATIO      (1.0/1.0)
  52.  
  53.  
  54.  
  55. /*
  56. *********************************************************************************************************
  57. *                                             EXPORTED_TYPES
  58. *********************************************************************************************************
  59. */
  60.  
  61. // Speed data structure 
  62. typedef struct
  63. {
  64.   int32_t x; // mm
  65.   int32_t y; // mm
  66.   float z; // degree
  67.   
  68. }Point;
  69.  
  70. typedef struct
  71. {
  72.   float Vx;
  73.   float Vy;
  74.   float Vz;
  75.   
  76. }Velocity; // unit: mm/s, rad/s
  77.  
  78. typedef struct
  79. {
  80. Velocity curSpeed; // (mm/s, mm/s, rad/s)
  81. Velocity expSpeed;
  82. Velocity outSpeed;
  83. Point expPos; // (mm, mm, rad)
  84.   
  85. }MOTION;
  86.  
  87. typedef struct
  88. {
  89.   float pos_x1; // mm
  90.   float pos_x2; // mm
  91.   float pos_z; // degree
  92.   
  93. }US;
  94.  
  95. // Position data structure
  96. typedef struct
  97. {
  98.   float pos_x; // mm
  99.   float pos_y; // mm
  100.   float zangle; // degree
  101.   float xangle; // degree
  102.   float yangle; // degree
  103.   float w_z;
  104.   float theta; // rad
  105.   
  106. }POSE;
  107.  
  108. // Laser pos
  109. typedef struct
  110. {
  111.   float pos_x; // mm
  112.   float pos_y; // mm
  113.   float pos_z; // degree
  114.   
  115. }LASER;
  116.  
  117. // Move method
  118. typedef enum
  119. {
  120. P2P = 0,
  121. MULTI,
  122. PCL,
  123.   
  124. }MOVE;
  125.  
  126. // Control method
  127. typedef enum
  128. {
  129. MANUAL = 0,
  130. AUTO,
  131.   
  132. }CTRL;
  133.  
  134. typedef enum // 机器人当前所在位置
  135. {
  136. Start = 0,
  137. LZ1, // 彩球
  138. LZ2, // 金球
  139. MeetPoint1,
  140. MeetPoint2,
  141.   
  142. }BOT;
  143.  
  144. typedef struct
  145. {
  146. MOVE RunMode;
  147. CTRL OP;
  148. BOT  State;
  149.   
  150.   void (*Move2Point) (uint8_t);
  151.   void (*WaitREADY) (void);
  152.   void (*DONE) (void);
  153.   
  154. }ACT;
  155.  
  156. typedef struct
  157. {
  158.   char ID;
  159.   int16_t current; // robomodule反馈的数据
  160.   int16_t velocity;
  161.   int32_t position;
  162.   int32_t realposition;
  163.   
  164. }BDC;
  165.  
  166.  
  167. // Accleration 
  168. typedef struct
  169. {
  170.   float ax;
  171.   float ay;
  172.   float az;
  173.   
  174. }Accelerate;
  175.  
  176. typedef struct
  177. {
  178.   /* Velocity profile parameter */
  179. Point qA;               // Start point,     uint: mm
  180. Point qB;               // End Point,       uint: mm
  181. Point delAB;			//delAB 			unit:mm
  182.   uint8_t end;
  183.   int32_t Dist;			// 	Distance		uint: mm
  184.   int32_t Acc;
  185.   int32_t Dec;
  186.   int32_t Vlin;          // mm/s, rad/s
  187.   int32_t AccTime;
  188.   int32_t Z_AccTime;
  189.   float 	DecTheta;
  190.   float 	AccTheta;
  191.   float 	Wz;
  192.   float	Z_Acc;
  193.   float	Z_Dec;
  194.   float	Theta;
  195.   float 	DecDist;
  196. }PclData;
  197.  
  198. typedef struct
  199. {
  200.   int32_t Motor_V;		  // 0.1 enc counts/sec
  201.   int16_t Motor_Torq;		// rated torque / 1000
  202.   
  203.   int32_t Target_V;
  204.   
  205. }Motor_Data;
  206. typedef struct
  207. {
  208. float L;//边长的一半，常数
  209. float Vx;//输入，action坐标系下的速度
  210. float Vy;//输入，action坐标系下的速度
  211. float omega;//输入，action坐标系下的角速度
  212. float alfa;//车头定位
  213. float R;//总转向半径
  214. float r[4];//轮半径
  215. float theta[4];//轮方向
  216. float V[4];//轮速度
  217. int halt;//停止移动指令
  218. }LUN;
  219.  
  220. extern LUN     Wheel3;
  221. extern LUN     Wheel4;
  222. extern PclData  PclBlock;
  223.  
  224.  
  225. /*
  226. *********************************************************************************************************
  227. *                                            GLOBAL VARIABLES
  228. *********************************************************************************************************
  229. */
  230.  
  231. ROBOT_CTRL_EXT MOTION     RoboMotion;
  232. ROBOT_CTRL_EXT POSE       RobotPos;
  233. ROBOT_CTRL_EXT POSE       RobotExpPos;
  234. ROBOT_CTRL_EXT US         RobotPosUS;
  235. ROBOT_CTRL_EXT LASER      RobotPosition;
  236. ROBOT_CTRL_EXT ACT        RobotCTRL;
  237. ROBOT_CTRL_EXT Point      COULMN_POS[40];
  238. ROBOT_CTRL_EXT Point      COULMN_ForestPOS[20];
  239. ROBOT_CTRL_EXT Point      JCC[15];
  240. ROBOT_CTRL_EXT BDC        BrushDC;
  241. ROBOT_CTRL_EXT CTRL       mode;
  242.  
  243. ROBOT_CTRL_EXT char      PIDflag;
  244. ROBOT_CTRL_EXT char      FeedbackData;
  245.  
  246. CHASSIS_EXT Motor_Data Wheel[WHEELNUM+1]; // Index 0 is reserved
  247. CHASSIS_EXT Motor_Data Target_V;
  248.  
  249. /*
  250. *********************************************************************************************************
  251. *                                           FUNCTION PROTOTYPES
  252. *********************************************************************************************************
  253. */
  254. float trans_PI(float n);
  255. void Robot_Motion_Init (void);
  256. void Robot_Speed_Update (void);
  257. void Robot_Speed_Ctrl (void);
  258. void SyncSignal (void);
  259.  
  260. void Pos_Paracal(PclData *profileData,uint8_t col);
  261. void Robot_Pos_Ctrl (void);
  262. void Chassis_Init (void);
  263. void SpeedCtrl (void);
  264. void SendWheel_Vel (void);
  265. void Send_Velocity (Motor_Data *wheel);
  266. void Formula_4Omni (Velocity *robo);
  267. float vision_turn(float *abc_data);
  268. /*
  269. *********************************************************************************************************
  270. *                                               MODULE END
  271. *********************************************************************************************************
  272. */
  273.  
  274. extern int Speed_Limit; // 最大速度
  275. extern char flag_brake;
  276. extern float R_x;
  277. extern float R_y;

文章知识点与官方知识档案匹配，可进一步学习相关知识
算法技能树首页概览 64136 人正在系统学习中

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.xdnf.cn/news/1559627.html

如若内容造成侵权/违法违规/事实不符，请联系一条长河网进行投诉反馈，一经查实，立即删除！

相关文章

Llama3 AI应用开发实战指南

Llama3 AI应用开发实战指南

引言 大环境不好的情况下&#xff0c;程序员如何生存&#xff0c;AI大模型开发是一个好的选择和尝试。所谓技多不压身。Llama3&#xff0c;作为近年来备受瞩目的开源大模型之一&#xff0c;以其强大的自然语言处理能力吸引了众多开发者的关注。这篇文章将通过一系列实战步骤&a…
阅读更多...
Anthropic Message Batches API 满足批量处理大量请求

Anthropic Message Batches API 满足批量处理大量请求

现在开发的系统有大量知识汇总统计、跑批处理需求的同学可以尝试一下&#xff0c;看看能不能解决自己目前的问题~~ 可能是一个解决方案 Anthropic 推出的 Message Batches API &#xff0c;专门用于帮助开发者批量处理大量请求。它的主要目的是通过一次性处理大量非实时任务&a…
阅读更多...
阿里云 CDN如何缓解ddos攻击

阿里云 CDN如何缓解ddos攻击

在网络安全日益重要的今天&#xff0c;DDoS攻击已成为企业面临的主要威胁之一。阿里云CDN&#xff08;内容分发网络&#xff09;以其强大的防护能力&#xff0c;成为抵御DDoS攻击的利器。九河云来和大家聊聊阿里云 CDN是如何缓解ddos攻击的吧。 首先&#xff0c;阿里云CDN通过…
阅读更多...
基于双波长AWG的窄线宽外差拍频激光器

基于双波长AWG的窄线宽外差拍频激光器

摘要&#xff1a;基于阵列波导光栅的多波长激光源已被证明可以同时提供多个波长和较窄的光线宽。为了产生毫米波信号&#xff0c;我们开发了两种不同的激光结构&#xff0c;并使用光子集成电路进行了单片集成。在这项工作中&#xff0c;我们报告了毫米波范围内的外差信号特性。…
阅读更多...
电脑屏保设置教程 好看的电脑屏保应该怎么设置？

电脑屏保设置教程 好看的电脑屏保应该怎么设置？

一、电脑自带的屏保设置&#xff0c;主题少&#xff0c;操作复杂&#xff1b; 你需要选择一个合适的屏保。在Windows系统中&#xff0c;你可以通过以下步骤找到合适的屏保&#xff1a; 右键点击桌面空白处&#xff0c;选择“个性化”&#xff1b; 在“个性化”设置中&#x…
阅读更多...
leetcode:反转字符串II

leetcode:反转字符串II

题目链接 string reverse(string s1) {string s2;string::reverse_iterator rit s1.rbegin();while (rit ! s1.rend()){s2 *rit;rit;}return s2; } class Solution { public:string reverseStr(string s, int k) {string s1;int i 0;//标记字符串下标int j 0;int length …
阅读更多...
【Oracle进阶】_001.SQL基础查询_查询语句

【Oracle进阶】_001.SQL基础查询_查询语句

课 程 推 荐我 的 个 人 主 页&#xff1a;&#x1f449;&#x1f449; 失心疯的个人主页 &#x1f448;&#x1f448;入 门 教 程 推 荐 &#xff1a;&#x1f449;&#x1f449; Python零基础入门教程合集 &#x1f448;&#x1f448;虚 拟 环 境 搭 建 &#xff1a;&#x1…
阅读更多...
第 2 章 基础支持层（上）

第 2 章 基础支持层（上）

2.1 解析器模块 常见的 XML 处理方式 DOM&#xff0c;基于树形结构的 XML 解析方式&#xff0c;它会将整个 XML 文档读入内存并构建一个 DOM 树&#xff0c;基于这棵树形结构对各个节点&#xff08;Node&#xff09;进行操作。 SAX&#xff0c;基于事件模型的 XML 解析方式&a…
阅读更多...
【无人水面艇路径跟随控制10】（Matlab）USV代码阅读：testUSV仿真无人水面艇在一定时间内的运动，使用欧拉法对状态进行积分，并绘制仿真结果

【无人水面艇路径跟随控制10】（Matlab）USV代码阅读：testUSV仿真无人水面艇在一定时间内的运动，使用欧拉法对状态进行积分，并绘制仿真结果

【无人水面艇路径跟随控制10】&#xff08;Matlab&#xff09;USV代码阅读&#xff1a;仿真无人水面艇在一定时间内的运动&#xff0c;使用欧拉法对状态进行积分&#xff0c;并绘制仿真结果 写在最前面testUSV.m总结代码详解1. **初始化部分**2. **仿真循环**3. **仿真数据提取…
阅读更多...
【docker】存储之目录挂载和卷映射

【docker】存储之目录挂载和卷映射

一、前言 之所以讲Docker存储的目录挂载和卷映射&#xff0c;是因为之前我们在做关于修改下载的镜像的内容的时候&#xff0c;我们会发现&#xff0c;这个修改的过程比较复杂的&#xff0c;我们需要多个指令&#xff0c;层层深入。而且如果我们的容器一旦销毁后重新启动&#x…
阅读更多...
全网首创Windows Powershell 批量创建、重命名和拷贝文件夹和文件到指定目录

全网首创Windows Powershell 批量创建、重命名和拷贝文件夹和文件到指定目录

哈喽大家好&#xff0c;欢迎来到虚拟化时代君&#xff08;XNHCYL&#xff09;。 “ 大家好&#xff0c;我是虚拟化时代君&#xff0c;一位潜心于互联网的技术宅男。这里每天为你分享各种你感兴趣的技术、教程、软件、资源、福利…&#xff08;每天更新不间断&#xff0c;福利…
阅读更多...
【linux】冯诺依曼架构

【linux】冯诺依曼架构

&#x1f525;个人主页&#xff1a;Quitecoder &#x1f525;专栏&#xff1a;linux笔记仓 目录 01.冯诺依曼体系结构02.操作系统&#xff08;Operator System&#xff09;如何理解“管理”操作系统中实现“管理的先描述再组织” 03.系统调用与库函数系统调用库函数 01.冯诺依…
阅读更多...
工作笔记【五】——媒体查询

工作笔记【五】——媒体查询

更新一下今天学到的东西——媒体查询&#xff08;浅学一下~&#xff09; 今天的任务是做一个网站的footer&#xff0c;要求在类似手机的设备打开时&#xff0c;footer元素竖向排列&#xff0c;在pc类的设备打开时&#xff0c;footer元素横向排列。 PC端&#xff1a; 手机端&a…
阅读更多...
SpringBoot+Vue智能社区服务小程序

SpringBoot+Vue智能社区服务小程序

SpringBootVue智能社区服务小程序 SpringBootVue智能社区服务小程序 项目描述 智能社区服务小程序的前台小程序是一个集成多功能的综合性平台&#xff0c;旨在提供便捷、高效的社区服务。以下是关于各个功能的简单介绍&#xff1a; 用户管理&#xff1a;用户管理模块负责社区…
阅读更多...
你知道C++多少——栈和队列

你知道C++多少——栈和队列

&#x1f308;个人主页&#xff1a;小新_- &#x1f388;个人座右铭&#xff1a;“成功者不是从不失败的人&#xff0c;而是从不放弃的人&#xff01;”&#x1f388; &#x1f381;欢迎各位→点赞&#x1f44d; 收藏⭐️ 留言&#x1f4dd; &#x1f3c6;所属专栏&#xff1…
阅读更多...
数据库管理平台应该具备哪些功能

数据库管理平台应该具备哪些功能

企业数据的的扁平一体化管理越来越普及。 一个企业如果想要做好数据扁平一体化管理的核心问题是如何高效地管理这些资源。构建一个能够完美整合、系统化组织数据&#xff0c;并提供直观可视分析界面的数据库管理平台&#xff0c;便是企业加速数字化转型的必备工具。 假如公司有…
阅读更多...
json格式的post请求目前不行, 要换成form表单形式的post请求怎么改

json格式的post请求目前不行, 要换成form表单形式的post请求怎么改

问: 下面是我的代码 export function fetchDeleteList<T>(agentSessionId: string) {return post<T>({url: http://192.168.0.116:8089/pipe-ics/agent/delete,method: post,data: { agentSessionId },}) } 目前是json格式的post请求, 目前不行, 要换成form表单…
阅读更多...
详解计算机组成原理中的层次化存储

详解计算机组成原理中的层次化存储

第5章 大而快&#xff1a;层次化存储 5.1 引言 局部性原理&#xff1a;在任意一段时间内&#xff0c;程序都只会访问地址空间中相对较小的一部分内容&#xff0c;就如你只会查阅图书馆的一部分藏书一样。 时间局部性&#xff1a;某个数据被访问&#xff0c;在不久的将来可能…
阅读更多...
CIME2025深圳国际热管理材料与设备展览会（2025.6.25-27）

CIME2025深圳国际热管理材料与设备展览会（2025.6.25-27）

CIME2025深圳国际热管理材料与设备展览会&#xff08;2025.6.25-27&#xff09; 2025.6.25-27 深圳国际会展中心 次世代热管理解决方案航天器热控分系统产品、基站环境级/机柜级热控产品以及基于环路热管的CPU、GPU、IGBT、T/R组件超级散热器其系统热阻、均温性、传热能力、逆…
阅读更多...
递归 深搜 回溯练习

递归 深搜 回溯练习

递归 深搜 回溯 题目一. 全排列II1. 题⽬链接&#xff1a;2. 题⽬描述&#xff1a;3. 解法&#xff1a;4.代码 题目二. 电话号码的字⺟组合1. 题⽬链接&#xff1a;2. 题⽬描述&#xff1a;3. 解法&#xff1a;4.代码 题目三. 括号⽣成&#xff08;medium&#xff09;1. 题⽬链…
阅读更多...
最新文章

Copyright @ 2022~2023