axis-1_3 完整包

axis 完整JAR包
  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
void ControlComply::BiaAngleCalculate(vector<XYZ_COOR_S> path_list, CONTROL_PARAM_IN para_in, robot::control_msg& para_out) { float distance_temp; int new_key_point = 0; XYZ_COOR_S xyz_temp; float delta_x[2], delta_y[2]; float min_distance = 100; int size = path_list.size(); float cur_x = para_in.cur_position.x_axis; float cur_y = para_in.cur_position.y_axis; float cur_head = para_in.cur_position.heading; for (int i = 0; i < size; i++) { xyz_temp = path_list.at(i); distance_temp = sqrt((xyz_temp.x_axis - cur_x) * (xyz_temp.x_axis - cur_x) + (xyz_temp.y_axis - cur_y) * (xyz_temp.y_axis - cur_y)); if (min_distance > distance_temp) { min_distance = distance_temp; new_key_point = i % size; } } // std::cout<<"00000000000000000000000000000 key ="<<new_key_point<<std::endl; // std::cout<<"cur = "<<cur_x<<","<<"y = "<<cur_y<<","<<"xyz = "<<xyz_temp.x_axis<<","<<"y = // "<<xyz_temp.y_axis<<std::endl; mKeyPoint = new_key_point; para_out.preCurve = path_list.at(mKeyPoint).curvature; if (path_list.at(path_list.size() - 3).curvature > para_out.preCurve) para_out.preCurve = path_list.at(path_list.size() - 3).curvature; delta_x[0] = cur_x - path_list.at(new_key_point).x_axis; delta_y[0] = cur_y - path_list.at(new_key_point).y_axis; delta_x[1] = path_list.at((new_key_point + 2) % size).x_axis - path_list.at(new_key_point).x_axis; delta_y[1] = path_list.at((new_key_point + 2) % size).y_axis - path_list.at(new_key_point).y_axis; distance_temp = delta_x[1] * delta_y[0] - delta_y[1] * delta_x[0]; if (distance_temp > 0) para_out.biaDistance = sqrtf(delta_x[0] * delta_x[0] + delta_y[0] * delta_y[0]); else para_out.biaDistance = -1 * sqrtf(delta_x[0] * delta_x[0] + delta_y[0] * delta_y[0]); para_out.preAngleDev = 0; }
07-17
这段代码定义了一个名为`BiaAngleCalculate`的函数,它接受三个参数:`path_list`(存储XYZ坐标的向量)、`para_in`(包含控制参数的结构体)和`para_out`(用于输出结果的结构体)。 首先,定义了一些局部变量,包括`distance_temp`(临时距离变量)、`new_key_point`(最近路径点的索引)、`xyz_temp`(临时存储XYZ坐标的结构体)、`delta_x`和`delta_y`(用于计算偏差的变量)、`min_distance`(最小距离值)以及`size`、`cur_x`、`cur_y`和`cur_head`(从输入参数中获取的当前位置和航向角)。 然后,使用一个循环遍历路径点向量中的每个元素。在循环中,计算当前路径点与当前位置的距离,并将其与最小距离值进行比较。如果当前距离更小,则更新最小距离值,并将最近路径点的索引设置为当前索引。 接下来,将最近路径点的索引保存在成员变量`mKeyPoint`中,并将该路径点的曲率值赋值给输出参数`para_out.preCurve`。如果路径向量中倒数第三个路径点的曲率值大于`preCurve`,则将其赋值给`preCurve`。 然后,计算偏差的变量`delta_x`和`delta_y`,用于后续计算。其中,`delta_x[0]`和`delta_y[0]`表示当前位置与最近路径点的X和Y坐标的差值,`delta_x[1]`和`delta_y[1]`表示下一个路径点与最近路径点的X和Y坐标的差值。 接着,计算当前位置与最近路径点连线与最近路径点与下一个路径点连线之间的向量叉乘。如果结果大于0,则表示当前位置在路径的左侧,将距离设置为正值;否则,将距离设置为负值。 最后,将输出参数`para_out.biaDistance`设置为当前位置与最近路径点的距离,并将输出参数`para_out.preAngleDev`设置为0。 总结来说,这段代码定义了一个函数,用于计算当前位置与路径之间的偏差值。它找到离当前位置最近的路径点,并计算当前位置与该路径点之间的距离和角度偏差,并将结果存储在输出参数中。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值