Get best of the week

使用RobotC语言的超声波测距仪和编码器实现确定物体宽度的算法的实现

美好的一天！

今天，我想分享使用RobotC编程语言（使用VEX EDR平台）使用超声波测距仪和编码器确定物体宽度的算法的实现。

史前史


最近，有人为我提供了使用RobotC编程语言解决有趣问题的大师班。在主讲课上，我考虑了确定对象宽度的任务，我想在本出版物中分享该算法的实现。

元素基础


作为实验训练场，我将机器人虚拟世界环境用于VEX平台，虚拟机器人模型VEX Launcher，VRC转折点竞赛元素的虚拟模型（图1）和最简单的数学公式（用于确定已知圆心角的圆的弦长）。

图片
图1。机器人虚拟训练场

任务


-仅使用安装在机器人上的超声波测距仪和编码器确定球的直径

理论基础


为了确定物体的宽度，我使用了辐射图的概念（图2）。

图片
图2.超声波测距仪的灵敏度和方向图

本质上，有必要确定在图的极点处的切线之间从最右位置转到最左位置时形成的角度。
考虑图3。

图片
图3。算法几何形状

我们将根据角度的半径与机器人旋转的弧度之间的关系搜索弧的中心角。最终的宽度值将对应于圆弧的m-和弦。

输入参数


作为输入参数，我选择了：

  • dist = 100 mm（机器人停止测量的距离，该参数是根据对象周围是否存在其他对象而选择的，这些对象可能会落入传感器的视线并干扰测量）
  • d_wheel = 102毫米（机器人的轮子直径）
  • d_ball = 102毫米（实际球直径）

确定了一个错误作为此任务的输出参数，它将指示我们的方法和所选参数的准确性：

  • 错误=绝对（d_ball_exper-d_ball）

程序运算算法


确定了算法的以下阶段。

  1. 运动到物体。
  2. 旋转到对象的最右点。
  3. 旋转到对象的最左点。
  4. 使用从编码器获得的读数计算角度。
  5. 错误计算。

RobotC算法的实现


首先，我们需要初始化虚拟机器人的标准配置：

#pragma config(StandardModel, "VEX Launchbot")

我们实现了一个功能，负责将机器人旋转到最右侧和/或最左侧。该函数在转一圈后返回编码器，该编码器将用于进一步的计算：

int turn_to_over_object(int leftSpeed, int rightSpeed, int distance, bool need_object, bool left_side)
{
if(need_object == true)
{
if(left_side == true)
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}
else
{
if(left_side == true)
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}

return getMotorEncoder(leftMotor);
}

让我们仔细看一下函数参数：

  • leftSpeed-左驱动器速度
  • rightSpeed-正确的驱动速度
  • rightSpeed-正确的驱动速度
  • distance-到机器人停止的物体的距离
  • need_object-运动模式：如果在传感器视场中需要某个对象，则为True，如果需要则为False
  • left_side-机器人的旋转方向。正确-左转，错误-右转

如果值Need_object = True，则机器人将旋转直到物体与机器人保持一定距离（在本例中为100 mm）。

如果值Need_object = False，则机器人将旋转直到对象与机器人之间的距离为（距离+ 1）。单位参数总计会影响确定传感器视场中是否存在物体的最终精度（可以通过将该参数作为函数参数来改进程序）。

让我们编写程序的主体：

task main()
{
int d_ball = 10.2; //sm
int d_wheel = 10.2;

int dist = 10; //sm
//  
setMotor(armMotor, -50);
wait(1);

while(SensorValue[sonarSensor] > 10)
{
setMultipleMotors(50, leftMotor, rightMotor, frontLeftMotor, frontRightMotor);
}
stopAllMotors();
int enc = 0;
enc = turn_to_over_object(50, 50, dist, false, false);
enc = turn_to_over_object(50, 50, dist, true, true);
resetMotorEncoder(leftMotor);
enc = turn_to_over_object(50, 50, dist, false, true);
stopAllMotors();

float teta = abs(enc) * PI * d_wheel * d_wheel / (dist * 627);
float d_ball_exper = 2 * dist * sin(teta / 2);
	
float error = abs(d_ball_exper - d_ball);
while(1)
{
}
}

第一个while循环运行直到机器人到达指定距离为止。接下来，我们需要一一调用上面编写的函数，以测量最右边和最左边位置之间的编码器读数（请注意，在测量之前，必须将编码器重置为零）。

为了得到角度公式，我受以下公式的指导：
-enc = N * 627，其中
    N是车轮的转数，
    enc是编码器读数，
    627是编码器分辨率=> express N；
-S = N * l，其中
    l是轮子的长度，l = PI * d_wheel * d_wheel =>我们用N代替l => S = enc * PI * d_wheel * d_wheel / 627;
-另一方面，S = dist * teta，其中
    S是机器人
    旋转的弧度，teta是所需的圆心角；
-根据上述数据，我们表示teta => teta = enc * PI * d_wheel * d_wheel /（627 * dist）

要找到等于所需球直径的弦，我们使用公式：
    d_ball_exper = 2 * dist * sin（teta / 2）

为了在调试器中查看接收到的值，必须在程序末尾进行无尽的while循环。

汇总数据


在虚拟字段上获得了以下球直径和误差值：

    d_ball_exper = 10.6 cm
    误差= 0.4 cm

如您所见，所获得的测量宽度的方法给出的误差值很小，对于所使用传感器的精度等级来说是可以接受的。可以通过选择dist参数的值来减小误差值（例如，以机器人以一定距离改变为dist的步长开始循环，或者使用PID控制器来减少误差值），这将减少制动过程中机器人惯性对编码器最终值的影响。

前景展望


将来，我将考虑使用机器人传感器确定环境参数的其他方法。

More articles:


All Articles