活动介绍

$ rocker --nvidia --x11 --volume .:/sim_ws/src/f1tenth_gym_ros -- f1tenth_gym_ros

时间: 2024-10-23 10:01:22 浏览: 66
看起来您在尝试使用Rocker这个容器平台运行一个名为f1tenth_gym_ros的项目,并且设置了几个特定选项: 1. `--nvidia`:这表示您希望在容器中使用NVIDIA GPU支持,以便于运行需要GPU资源的仿真环境,如图形密集型机器学习训练或游戏等。 2. `--x11`:通常用于在容器内启用X11协议,使得可以显示图形界面,这对于依赖图形用户界面的应用非常重要。 3. `--volume .:/sim_ws/src/f1tenth_gym_ros`: 这是一个挂载选项,将主机目录`.`(当前工作目录)映射到容器内的`/sim_ws/src/f1tenth_gym_ros`路径,这样可以在容器内部访问和修改本地的`f1tenth_gym_ros`项目的源码文件。 4. `f1tenth_gym_ros`:最后的部分就是实际要运行的服务或命令,可能是基于ROS(机器人操作系统)的一个赛车模拟环境。 简而言之,这是一个命令行指令,用于启动一个包含NVIDIA GPU、X11支持的Rocker镜像,运行带有f1tenth_gym_ros应用的工作空间,允许交互式图形界面并提供本地目录的读写权限。
相关问题

gany@WP:~/1$ rosrun tf tf_echo map rb_0/base_link Failure at 3728.842000000 Exception thrown:"map" passed to lookupTransform argument target_frame does not exist. The current list of frames is: Frame rb_0/axle_link exists with parent rb_0/chassis_link. Frame rb_0/chassis_link exists with parent rb_0/base_link. Frame rb_0/front_cover_link exists with parent rb_0/chassis_link. Frame rb_0/front_laser exists with parent rb_0/chassis_link. Frame rb_0/front_lights_link exists with parent rb_0/chassis_link. Frame rb_0/imu_link exists with parent rb_0/chassis_link. Frame rb_0/left_side_cover_link exists with parent rb_0/chassis_link. Frame rb_0/mid_mount exists with parent rb_0/base_link. Frame rb_0/rear_cover_link exists with parent rb_0/chassis_link. Frame rb_0/rear_laser exists with parent rb_0/chassis_link. Frame rb_0/rear_lights_link exists with parent rb_0/chassis_link. Frame rb_0/rear_rocker_link exists with parent rb_0/axle_link. Frame rb_0/right_side_cover_link exists with parent rb_0/chassis_link. Frame rb_0/top_link exists with parent rb_0/chassis_link. Frame rb_1/axle_link exists with parent rb_1/chassis_link.

<think>好的,用户遇到了TF变换超时的问题,具体错误是执行`rosrun tf tf_echo map rb_0/base_link`时显示map帧不存在,并且当前的帧列表里只有rb_0和rb_1相关的子帧,没有map到odom或base_link的连接。我需要先理解用户的问题背景。用户之前已经得到了一份详细的排查方案,现在他们可能正在按照步骤操作,但依然没有解决,所以提供了新的错误信息。 首先,用户提供的错误信息显示,当尝试获取从map到rb_0/base_link的变换时,系统提示map帧不存在。查看现有的帧列表,发现所有存在的帧都是机器人rb_0和rb_1下的子部件,比如chassis_link、axle_link等,但没有map帧,也没有odom帧。这说明整个TF树中,map帧根本没有被发布出来,或者命名空间不正确,导致无法连接到机器人的base_link。 根据之前的回答,可能的原因包括地图服务器未正确启动、AMCL配置错误、静态变换未正确发布,或者多机器人命名空间问题。但用户现在的情况是map帧完全不存在,所以最可能的问题是map_server节点没有运行,或者静态变换发布不正确,或者命名空间导致map帧被错误地发布到了其他位置。 接下来,我需要考虑用户可能的操作环境。用户可能在多机器人系统中使用命名空间,例如rb_0和rb_1,因此map到odom的变换可能被发布到了全局命名空间,或者需要加上命名空间前缀。但根据错误信息,现有的帧列表里根本没有map帧,这说明map帧没有被任何节点发布,或者发布的帧名称不正确,比如可能被错误地添加了命名空间前缀。 检查步骤方面,首先应该确认map_server节点是否正常运行,并正确加载了地图文件。如果map_server没有运行,或者地图文件路径错误,map帧就不会被发布。其次,检查是否有静态变换发布map到odom,例如使用static_transform_publisher,但可能在启动该节点时参数错误,比如目标帧写成了rb_0/odom而不是odom,导致map到odom的变换存在于rb_0命名空间下,而其他节点在查找全局map帧时找不到。 另外,用户可能在多机器人系统中,每个机器人有自己的命名空间,但map帧应该是全局的,不属于任何命名空间。如果静态变换发布时错误地将map帧发布到了rb_0的命名空间下,那么实际上的帧名称会是rb_0/map,而其他节点在查找map时会失败,因为正确的帧应该是map。 所以,可能的解决方案包括: 1. 检查map_server是否正常运行,并正确发布map帧。 2. 检查静态变换发布的参数是否正确,确保源和目标帧是map和odom,而不是带有命名空间前缀的。 3. 确认AMCL节点的配置中global_frame_id设置为map,并且odom_frame_id正确对应到机器人的odom帧(如rb_0/odom)。 4. 使用命令行工具如rostopic和rosnode来检查节点和话题的状态,确保所有相关节点都在运行。 此外,用户提供的现有帧结构显示,rb_0/base_link存在,但其父级是rb_0/chassis_link,这说明机器人的TF树可能存在内部结构,但没有连接到odom或map。因此,需要确保odom到base_link的变换被正确发布,通常由里程计节点(如wheel odometry或AMCL)发布。 总结来说,用户的问题核心在于map帧未被正确发布到TF树中,导致无法形成从map到base_link的完整变换链。需要逐一排查map_server、静态变换发布、AMCL配置以及命名空间是否正确。</think>$\textbf{TF变换故障深度诊断报告}$ --- ### $\textbf{一、核心问题定位}$ 通过输出日志可确认: 1. **map帧完全缺失** TF树中未检测到任何与`map`相关的坐标系(包括`map→odom`或`map→rb_X/odom`链路) 2. **机器人本体链路完整** `rb_0/base_link`到各子部件的变换链正常(如`chassis_link→axle_link→...`) 3. **多机命名空间隔离有效** `rb_0`与`rb_1`的坐标系已正确分离 --- ### $\textbf{二、关键故障环节}$ $$\boxed{\text{地图服务未正确发布map帧}} \cup \boxed{\text{map→odom静态变换缺失}}$$ --- ### $\textbf{三、分步解决方案}$ $\textbf{步骤1:验证地图服务状态}$ ```bash # 查看当前活跃的节点 rosnode list | grep map_server # 检查地图话题数据 rostopic echo /map -n 1 # 应显示地图元数据 ``` $\rightarrow$ 若无输出,说明地图服务未启动。需检查启动文件: ```xml <!-- 正确的地图服务配置示例 --> <node pkg="map_server" type="map_server" name="map_server" args="$(find your_pkg)/maps/mymap.yaml"/> ``` $\textbf{步骤2:重建基础TF链路}$ 在全局命名空间发布静态变换(**注意无机器人前缀**): ```bash # 临时测试发布 rosrun tf static_transform_publisher 0 0 0 0 0 0 map odom 100 # 永久配置(写入launch文件) <node pkg="tf" type="static_transform_publisher" name="global_map_to_odom" args="0 0 0 0 0 0 map odom 100"/> ``` $\textbf{步骤3:AMCL全局坐标系绑定}$ 修改`amcl.launch`确保坐标系映射正确: ```xml <param name="global_frame_id" value="map" /> <!-- 必须为全局map --> <param name="odom_frame_id" value="rb_0/odom" /> <param name="base_frame_id" value="rb_0/base_link" /> ``` --- ### $\textbf{四、多机器人系统特殊配置}$ 对于多机协同场景,需建立分层TF结构: ``` 全局坐标系 └── map (静态) └── odom (全局) ├── rb_0/odom (局部) │ └── rb_0/base_link └── rb_1/odom (局部) └── rb_1/base_link ``` 实现方法: ```python # 动态发布全局odom到各机器人odom的变换 for robot in ["rb_0", "rb_1"]: tf_pub.sendTransform( (x, y, z), quaternion, rospy.Time.now(), f"{robot}/odom", # 子坐标系 "odom" # 父坐标系(全局odom) ) ``` --- ### $\textbf{五、验证方法}$ 1. **实时监控TF树** ```bash rosrun rqt_tf_tree rqt_tf_tree # 确认map出现在根节点 ``` ![正确TF树结构示意图](https://i.imgur.com/5GzQqKL.png) 2. **坐标变换数学验证** 执行坐标变换计算: $$ T_{map}^{base\_link} = T_{map}^{odom} \cdot T_{odom}^{rb\_0/odom} \cdot T_{rb\_0/odom}^{base\_link} $$ 使用实际数据验证矩阵连乘结果: ```bash rosrun tf tf_echo map rb_0/base_link # 现在应能输出有效变换 ``` --- ### $\textbf{六、典型错误对照}$ | 现象 | 数学表达 | 解决方案 | |------|---------|----------| | map帧未注册 | $\exists T_{map}^{odom} = \emptyset$ | 检查static_transform_publisher参数 | | 坐标系层级断裂 | $T_{parent}^{child} \notin \mathbb{TF}$ | 使用`view_frames.py`生成拓扑图 | | 时间不同步 | $t_{map} - t_{odom} > \Delta t_{max}$ | 在launch文件中添加`<param name="use_sim_time" value="true"/>` | --- 通过上述操作后再次运行`tf_echo`,应该能观察到完整的坐标变换数据。若问题仍未解决,请提供: 1. `rosnode info /map_server` 2. `rostopic hz /tf_static` 3. 完整的launch文件配置片段

[ INFO] [1741943669.752676608, 3041.543000000]: robotBaseFrame for force based move plugin: base_link [ INFO] [1741943669.783771496, 3041.543000000]: GazeboRosJointStatePublisher is going to publish joint: front_rocker [ INFO] [1741943669.783807882, 3041.543000000]: Starting GazeboRosJointStatePublisher Plugin (ns = /rb_0//)!, parent name: rb_0 [ WARN] [1741943669.785414887, 3041.544000000]: Failed to meet update rate! Took 3041.543999999999869 [ WARN] [1741943669.785626142, 3041.544000000]: Failed to meet update rate! Took 3041.543999999999869 [ WARN] [1741943669.786521836, 3041.545000000]: Failed to meet update rate! Took 3041.5250000000000909 [ WARN] [1741943669.787050378, 3041.546000000]: Failed to meet update rate! Took 3041.5259999999998399 [rb_0/urdf_spawner-9] process has died [pid 72005, exit code 1, cmd /opt/ros/noetic/lib/gazebo_ros/spawn_model -urdf -model rb_0 -param robot_description -x 0 -y 0 -z 0.1 -Y 0 __name:=urdf_spawner __log:=/home/gany/.ros/log/ba19632c-00b4-11f0-bcbb-4f7e799ac125/rb_0-urdf_spawner-9.log]. log file: /home/gany/.ros/log/ba19632c-00b4-11f0-bcbb-4f7e799ac125/rb_0-urdf_spawner-9*.log

<think>好的,我现在需要帮用户分析他们提供的ROS/Gazebo日志信息。首先,用户给了一个系统级指令,要求用中文回答,并且注意数学表达式的格式,但这个问题看起来主要涉及ROS和Gazebo的日志解析,可能不需要数学公式。不过还是要注意用户提到的格式要求,如果有用到的话。 首先,用户的信息中包含多个日志条目,包括INFO和WARN级别的消息,以及一个进程死亡的报告。我需要逐一分析这些条目,找出可能的问题原因。 第一条信息是关于robotBaseFrame设置为base_link,这可能涉及到力控移动插件的配置。用户可能在使用某个力控制的插件,比如用于机器人移动的插件,正确设置基座框架是重要的,这里显示的是base_link,看起来正常。 接下来是GazeboRosJointStatePublisher开始发布front_rocker关节的状态,这可能属于机器人的悬挂系统,比如六轮探测车之类的。这个插件用于将Gazebo中的关节状态发布到ROS的/joint_states话题,通常用于机器人状态反馈。需要注意命名空间是/rb_0//,这里可能有个双斜杠的问题,但可能只是打印时的格式问题,或者命名空间设置有小错误。 然后是多个关于无法满足更新率的警告,显示时间戳中的数值非常大,比如3041秒,这大约相当于50分钟。这显然有问题,因为Gazebo通常以实时或更快的速度运行。时间戳异常可能说明系统时间或仿真时间管理有问题。可能的原因是系统时钟问题,或者Gazebo在启动时遇到了某种延迟,导致仿真时间与实际时间不同步。 最后,urdf_spawner进程死亡,退出码1,这通常意味着在生成模型时发生了错误。可能的原因包括URDF文件有误,模型名称冲突,或者参数服务器中没有找到robot_description参数。需要检查spawn_model命令的参数是否正确,特别是-param robot_description是否存在,或者是否有权限问题导致无法读取参数。 接下来,我需要将这些分析结构化,给出用户可能的问题点。首先,时间戳异常的问题,可能是系统负载过高,或者Gazebo无法实时运行,导致仿真时间落后。其次,urdf_spawner失败,需要检查URDF的正确性以及参数是否正确设置。另外,命名空间的警告可能需要检查代码中的命名设置是否有误。 用户可能需要检查他们的URDF文件是否有语法错误,或者尝试手动运行spawn_model命令查看具体错误信息。同时,检查系统资源,确保Gazebo能够正常运行,没有性能瓶颈。此外,查看日志文件/home/gany/.ros/log/...中的详细错误信息,可能包含更具体的错误原因。 总结下来,用户的问题可能集中在URDF生成失败和Gazebo实时性能不足两个方面。需要引导用户检查相关配置和日志,以进一步诊断问题。</think>让我们逐步分析这个ROS/Gazebo日志信息: 一、核心信息解析 $$[ INFO] [1741943669.752676608, 3041.543000000]: robotBaseFrame\ for\ force\ based\ move\ plugin:\ base\_link$$ 这表示力控运动插件的基准坐标系被设置为$base\_link$,这是机器人运动控制的参考坐标系。 二、关节状态发布器配置 $$[ INFO] [1741943669.783771496, 3041.543000000]: GazeboRosJointStatePublisher\ is\ going\ to\ publish\ joint:\ front\_rocker$$ 说明机器人$rb\_0$的前摇杆关节(front_rocker)状态即将通过`/joint_states`话题发布,这是典型的轮式机器人(如六轮探测车)悬挂系统配置。 三、关键警告分析 ``` [ WARN] [...] Failed to meet update rate! Took 3041.543999... ``` 这些时间戳异常(3041秒≈50分钟)暗示: 1. 系统时间同步问题 2. Gazebo无法保持实时仿真速率 3. 硬件性能不足(常见于物理引擎计算过载) 四、致命错误诊断 ``` [rb_0/urdf_spawner-9] process has died [pid 72005, exit code 1...] ``` URDF模型生成失败的可能原因: 1. URDF文件存在语法错误(检查`robot_description`参数) 2. 模型命名冲突(已存在同名$rb\_0$模型) 3. 坐标参数异常(z=0.1可能引发碰撞检测失败) 4. 文件权限问题(日志路径/home/gany/.ros/...需要写入权限) 五、建议排查步骤 1. 验证URDF完整性: ```bash rosrun urdf_parser check_urdf your_robot.urdf ``` 2. 手动生成模型测试: ```bash rosrun gazebo_ros spawn_model -urdf -model test_bot -param robot_description ``` 3. 检查系统时钟同步: ```bash ntpq -p # 验证NTP服务状态 ``` 4. 监控硬件资源: ```bash htop # 观察CPU/内存使用情况 nvidia-smi # 检查GPU负载(如使用GPU加速) ``` 六、典型解决方案 - 降低仿真复杂度(简化碰撞体、减少多边形数量) - 增加Gazebo实时因子: ```xml <real_time_update_rate>1000</real_time_update_rate> ``` - 检查`robot_description`参数是否成功加载: ```bash rosparam get /robot_description | head -n 20 ``` 建议优先查看完整日志文件: ```bash less /home/gany/.ros/log/.../rb_0-urdf_spawner-9*.log ```
阅读全文

相关推荐

#include "control.h" #include "Lidar.h" float Move_X = 0, Move_Z = 0; // Ä¿±êËٶȺÍÄ¿±êתÏòËÙ¶È float PWM_Left, PWM_Right; // ×óÓÒµç»úPWMÖµ float RC_Velocity, RC_Turn_Velocity; // Ò£¿Ø¿ØÖƵÄËÙ¶È u8 Mode = Normal_Mode; // ģʽѡÔñ£¬Ä¬ÈÏÊÇÆÕͨµÄ¿ØÖÆÄ£Ê½ Motor_parameter MotorA, MotorB; // ×óÓÒµç»úÏà¹Ø±äÁ¿ int Servo_PWM = SERVO_INIT; // °¢¿ËÂü¶æ»úÏà¹Ø±äÁ¿ u8 Lidar_Detect = Lidar_Detect_ON; // µç´ÅѲÏßģʽÀ×´ï¼ì²âÕϰ­ÎĬÈÏ¿ªÆô float CCD_Move_X = 0.3; // CCDѲÏßËÙ¶È float ELE_Move_X = 0.3; // µç´ÅѲÏßËÙ¶È u8 Ros_count = 0, Lidar_flag_count = 0; u8 cnt = 0; Encoder OriginalEncoder; // Encoder raw data //±àÂëÆ÷ԭʼÊý¾Ý short Accel_Y, Accel_Z, Accel_X, Accel_Angle_x, Accel_Angle_y, Gyro_X, Gyro_Z, Gyro_Y; int forward_cnt = 800; int left_cnt = 120; int right_cnt = 125; int stop_cnt = 200; int stop_flag = 0; int mode_cnt = 0; int state_cnt = 0; int stop_protect = 0; /************************************************************************** Function: Control Function Input : none Output : none º¯Êý¹¦ÄÜ£º5ms¶¨Ê±ÖжϿØÖƺ¯Êý Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void forward(){ MotorA.Motor_Pwm = 1999; MotorB.Motor_Pwm = 2040; } void left(){ MotorA.Motor_Pwm = 0; MotorB.Motor_Pwm = 2050; } void right(){ MotorA.Motor_Pwm = 1999; MotorB.Motor_Pwm = 0; } void stop(){ MotorA.Motor_Pwm = 0; MotorB.Motor_Pwm = 0; } void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim == &htim5) { Get_KeyVal(); if(mode_cnt == 0) stop(); //ģʽһ if(mode_cnt == 1){ if(forward_cnt > 0){ forward(); forward_cnt--; // if(left_cnt > 0){ // left(); // left_cnt--; // if(right_cnt > 0){ // right(); // right_cnt--; } else{ stop(); } } //ģʽ¶þ else if(mode_cnt == 2){ if(forward_cnt > 0){ forward(); forward_cnt--; } else if(stop_cnt > 0 && forward_cnt == 0){ stop(); stop_cnt--; } else if(stop_cnt == 0 && stop_flag == 0){ forward_cnt = 800; stop_flag = 1; } else{ stop(); } } //ģʽÈý else if(mode_cnt == 3){ if(forward_cnt > 0 && state_cnt == 0){//µÚ1״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(right_cnt > 0 && state_cnt == 1){//µÚ2״̬£ºÓÒת right(); right_cnt--; } else if(forward_cnt > 0 && state_cnt == 2){//µÚ3״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(left_cnt > 0 && state_cnt == 3){//µÚ4״̬£º×óת left(); left_cnt--; } else if(forward_cnt > 0 && state_cnt == 4){//µÚ5״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(left_cnt > 0 && state_cnt == 5){//µÚ6״̬£º×óת left(); left_cnt--; } else if(forward_cnt > 0 && state_cnt == 6){//µÚ7״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else{ if(stop_protect == 0 && stop_cnt == 0){ stop_cnt = 100; stop_protect = 1; } else if(stop_cnt > 0){ stop(); stop_cnt--; } else if(stop_protect == 1 && stop_cnt == 0){ stop_protect = 0; if(state_cnt == 0){//״̬1Çл»×´Ì¬2 state_cnt++; right_cnt = 125; } else if(state_cnt == 1){//״̬2Çл»×´Ì¬3 state_cnt++; forward_cnt = 585; } else if(state_cnt == 2){//״̬3Çл»×´Ì¬4 state_cnt++; left_cnt = 120; } else if(state_cnt == 3){//״̬4Çл»×´Ì¬5 state_cnt++; forward_cnt = 585; } else if(state_cnt == 4){//״̬5Çл»×´Ì¬6 state_cnt++; left_cnt = 120; } else if(state_cnt == 5){//״̬6Çл»×´Ì¬7 state_cnt++; forward_cnt = 585; } } } } Set_Pwm(-MotorA.Motor_Pwm, MotorB.Motor_Pwm); // Çý¶¯µç»ú } } /************************************************************************** Function: Bluetooth_Control Input : none Output : none º¯Êý¹¦ÄÜ£ºÊÖ»úÀ¶ÑÀ¿ØÖÆ Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Bluetooth_Control(void) { if (Flag_Direction == 0) Move_X = 0, Move_Z = 0; // Í£Ö¹ else if (Flag_Direction == 1) Move_X = RC_Velocity, Move_Z = 0; // ǰ½ø else if (Flag_Direction == 2) Move_X = RC_Velocity, Move_Z = Pi / 2; // ÓÒǰ else if (Flag_Direction == 3) Move_X = 0, Move_Z = Pi / 2; // ÏòÓÒ else if (Flag_Direction == 4) Move_X = -RC_Velocity, Move_Z = Pi / 2; // ÓÒºó else if (Flag_Direction == 5) Move_X = -RC_Velocity, Move_Z = 0; // ºóÍË else if (Flag_Direction == 6) Move_X = -RC_Velocity, Move_Z = -Pi / 2; // ×óºó else if (Flag_Direction == 7) Move_X = 0, Move_Z = -Pi / 2; // Ïò×ó else if (Flag_Direction == 8) Move_X = RC_Velocity, Move_Z = -Pi / 2; // ×óǰ else Move_X = 0, Move_Z = 0; if (Car_Num == Akm_Car) { // Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed // °¢¿ËÂü½á¹¹Ð¡³µ×ª»»ÎªÇ°ÂÖתÏò½Ç¶È Move_Z = Move_Z * 2 / 10; } Move_X = Move_X / 1000; Move_Z = -Move_Z; // ת»»ÎªËÙ¶ÈתΪm/s } /************************************************************************** Function: PS2_Control Input : none Output : none º¯Êý¹¦ÄÜ£ºPS2ÊÖ±ú¿ØÖÆ Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void PS2_Control(void) { int LY, RX; // ÊÖ±úADCµÄÖµ int Threshold = 20; // ãÐÖµ£¬ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷ static u8 Key1_Count = 0, Key2_Count = 0; // ÓÃÓÚ¿ØÖƶÁȡҡ¸ËµÄËÙ¶È // ת»¯Îª128µ½-128µÄÊýÖµ LY = -(PS2_LY - 128); // ×ó±ßYÖá¿ØÖÆÇ°½øºóÍË RX = -(PS2_RX - 128); // ÓÒ±ßXÖá¿ØÖÆ×ªÏò if (LY > -Threshold && LY < Threshold) LY = 0; if (RX > -Threshold && RX < Threshold) RX = 0; // ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷ Move_X = (RC_Velocity / 128) * LY; // ËÙ¶È¿ØÖÆ£¬Á¦¶È±íʾËÙ¶È´óС if (Car_Num == Akm_Car) // °¢¿ËÂü³µ×ªÏò¿ØÖÆ£¬Á¦¶È±íʾתÏò½Ç¶È Move_Z = -(RC_Turn_Velocity / 128) * RX; else // ÆäËû³µÐÍתÏò¿ØÖÆ { if (Move_X >= 0) Move_Z = -(RC_Turn_Velocity / 128) * RX; // תÏò¿ØÖÆ£¬Á¦¶È±íʾתÏòËÙ¶È else Move_Z = (RC_Turn_Velocity / 128) * RX; } if (PS2_KEY == PSB_L1) // °´ÏÂ×ó1¼ü¼ÓËÙ£¨°´¼üÔÚ¶¥ÉÏ£© { if ((++Key1_Count) == 20) // µ÷½Ú°´¼ü·´Ó¦ËÙ¶È { PS2_KEY = 0; Key1_Count = 0; if ((RC_Velocity += X_Step) > MAX_RC_Velocity) // ǰ½ø×î´óËÙ¶È800mm/s RC_Velocity = MAX_RC_Velocity; if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È { if ((RC_Turn_Velocity += Z_Step) > MAX_RC_Turn_Bias) // תÏò×î´óËÙ¶È325 RC_Turn_Velocity = MAX_RC_Turn_Bias; } } } else if (PS2_KEY == PSB_R1) // °´ÏÂÓÒ1¼ü¼õËÙ { if ((++Key2_Count) == 20) { PS2_KEY = 0; Key2_Count = 0; if ((RC_Velocity -= X_Step) < MINI_RC_Velocity) // ǰºó×îСËÙ¶È210mm/s RC_Velocity = MINI_RC_Velocity; if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È { if ((RC_Turn_Velocity -= Z_Step) < MINI_RC_Turn_Velocity) // תÏò×îСËÙ¶È45 RC_Turn_Velocity = MINI_RC_Turn_Velocity; } } } else Key2_Count = 0, Key2_Count = 0; // ¶ÁÈ¡µ½ÆäËû°´¼üÖØÐ¼ÆÊý Move_X = Move_X / 1000; Move_Z = -Move_Z; // ËÙ¶ÈMove_XתΪm/s } /************************************************************************** Function: Get_Velocity_From_Encoder Input : none Output : none º¯Êý¹¦ÄÜ£º¶ÁÈ¡±àÂëÆ÷ºÍת»»³ÉËÙ¶È Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Velocity_From_Encoder(void) { // Retrieves the original data of the encoder // »ñÈ¡±àÂëÆ÷µÄԭʼÊý¾Ý float Encoder_A_pr, Encoder_B_pr; OriginalEncoder.A = Read_Encoder(Encoder1); OriginalEncoder.B = Read_Encoder(Encoder2); // Decide the encoder numerical polarity according to different car models // ¸ù¾Ý²»Í¬Ð¡³µÐͺžö¶¨±àÂëÆ÷ÊýÖµ¼«ÐÔ switch (Car_Num) { case Akm_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Diff_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Small_Tank_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Big_Tank_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; } // The encoder converts the raw data to wheel speed in m/s // ±àÂëÆ÷ԭʼÊý¾Ýת»»Îª³µÂÖËÙ¶È£¬µ¥Î»m/s MotorA.Current_Encoder = Encoder_A_pr * Frequency * Perimeter / 60000.0f; // 60000 = 4*500*30 MotorB.Current_Encoder = Encoder_B_pr * Frequency * Perimeter / 60000.0f; // 1560=4*13*30=2£¨Á½Â·Âö³å£©*2£¨ÉÏÏÂÑØ¼ÆÊý£©*»ô¶û±àÂëÆ÷13Ïß*µç»úµÄ¼õËÙ±È // MotorA.Current_Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Akm_wheelspacing//(4*13*30); // MotorB.Current_Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Akm_wheelspacing/Encoder_precision; } /************************************************************************** Function: Drive_Motor Input : none Output : none º¯Êý¹¦ÄÜ£ºÔ˶¯Ñ§Äæ½â Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ // Ô˶¯Ñ§Äæ½â£¬ÓÉxºÍyµÄËٶȵõ½±àÂëÆ÷µÄËÙ¶È,VxÊÇm/s,Vzµ¥Î»ÊǶÈ/s(½Ç¶ÈÖÆ) // °¢¿ËÂü³µVzÊǶæ»úתÏòµÄ½Ç¶È(»¡¶ÈÖÆ) void Get_Target_Encoder(float Vx, float Vz) { float MotorA_Velocity, MotorB_Velocity; float amplitude = 3.5f; // Wheel target speed limit //³µÂÖÄ¿±êËÙ¶ÈÏÞ·ù Move_X = target_limit_float(Move_X, -1.2, 1.2); Move_Z = target_limit_float(Move_Z, -Pi / 3, Pi / 3); if (Car_Num == Akm_Car) // °¢¿ËÂü³µ { // Ackerman car specific related variables //°¢¿ËÂüС³µ×¨ÓÃÏà¹Ø±äÁ¿ float R, ratio = 636.56, AngleR, Angle_Servo; // For Ackerman small car, Vz represents the front wheel steering Angle // ¶ÔÓÚ°¢¿ËÂüС³µVz´ú±íÓÒǰÂÖתÏò½Ç¶È AngleR = Vz; R = Akm_axlespacing / tan(AngleR) - 0.5f * Wheelspacing; // Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad // ǰÂÖתÏò½Ç¶ÈÏÞ·ù(¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È)£¬µ¥Î»£ºrad AngleR = target_limit_float(AngleR, -0.49f, 0.32f); // Inverse kinematics //Ô˶¯Ñ§Äæ½â if (AngleR != 0) { MotorA.Target_Encoder = Vx * (R - 0.081f) / R; MotorB.Target_Encoder = Vx * (R + 0.081f) / R; } else { MotorA.Target_Encoder = Vx; MotorB.Target_Encoder = Vx; } // The PWM value of the servo controls the steering Angle of the front wheel // ¶æ»úPWMÖµ£¬¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È Angle_Servo = -0.628f * pow(AngleR, 3) + 1.269f * pow(AngleR, 2) - 1.772f * AngleR + 1.573f; Servo_PWM = SERVO_INIT + (Angle_Servo - 1.572f) * ratio; // printf("%d\r\n",Servo_PWM); // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); Servo_PWM = target_limit_int(Servo_PWM, 800, 2200); // Servo PWM value limit //¶æ»úPWMÖµÏÞ·ù } else if (Car_Num == Diff_Car) // ²îËÙС³µ { if (Vx < 0) Vz = -Vz; else Vz = Vz; // Inverse kinematics //Ô˶¯Ñ§Äæ½â MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } else if (Car_Num == Small_Tank_Car) { if (Vx < 0) Vz = -Vz; else Vz = Vz; MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } else if (Car_Num == Big_Tank_Car) { if (Vx < 0) Vz = -Vz; else Vz = Vz; MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } } /************************************************************************** Function: Get_Motor_PWM Input : none Output : none º¯Êý¹¦ÄÜ£º×ª»»³ÉÇý¶¯µç»úµÄPWM Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Motor_PWM(void) { // ¼ÆËã×óÓÒµç»ú¶ÔÓ¦µÄPWM MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder); MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder); if (Mode == Normal_Mode || Mode == Measure_Distance_Mode) { // Â˲¨£¬Ê¹Æð²½ºÍÍ£Ö¹ÉÔ΢ƽ»¬Ò»Ð© MotorA.Motor_Pwm = Mean_Filter_Left(MotorA.Motor_Pwm); MotorB.Motor_Pwm = Mean_Filter_Right(MotorB.Motor_Pwm); } // ÏÞ·ù MotorA.Motor_Pwm = PWM_Limit(MotorA.Motor_Pwm, PWM_MAX, PWM_MIN); MotorB.Motor_Pwm = PWM_Limit(MotorB.Motor_Pwm, PWM_MAX, PWM_MIN); } /************************************************************************** Function: PWM_Limit Input : IN;max;min Output : OUT º¯Êý¹¦ÄÜ£ºÏÞÖÆPWM¸³Öµ Èë¿Ú²ÎÊý: IN£ºÊäÈë²ÎÊý max£ºÏÞ·ù×î´óÖµ min£ºÏÞ·ù×îСֵ ·µ»Ø Öµ£ºÏÞ·ùºóµÄÖµ **************************************************************************/ float PWM_Limit(float IN, int max, int min) { float OUT = IN; if (OUT > max) OUT = max; if (OUT < min) OUT = min; return OUT; } /************************************************************************** Function: Limiting function Input : Value Output : none º¯Êý¹¦ÄÜ£ºÏÞ·ùº¯Êý Èë¿Ú²ÎÊý£º·ùÖµ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ float target_limit_float(float insert, float low, float high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } int target_limit_int(int insert, int low, int high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } /************************************************************************** Function: Check whether it is abnormal Input : none Output : 1:Abnormal;0:Normal º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£º1£ºÒì³£ 0£ºÕý³£ **************************************************************************/ u8 Turn_Off(void) { u8 temp = Normal; Flag_Stop = KEY2_STATE; // ¶ÁÈ¡°´¼ü2״̬£¬°´¼ü2¿ØÖƵç»úµÄ¿ª¹Ø if (Voltage < 1000) // µç³ØµçѹµÍÓÚ10V¹Ø±Õµç»ú,LEDµÆ¿ìËÙÉÁ˸ LED_Flash(50), temp = Abnormal; else LED_Flash(200); // ÿһÃëÉÁÒ»´Î£¬Õý³£ÔËÐÐ if (Flag_Stop) temp = Abnormal; return temp; } /************************************************************************** Function: Data sliding filtering Input : data Output : Filtered data º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨ Èë¿Ú²ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý **************************************************************************/ float Mean_Filter_Left(float data) { u8 i; float Sum_Data = 0; float Filter_Data; static float Speed_Buf[FILTERING_TIMES] = {0}; for (i = 1; i < FILTERING_TIMES; i++) { Speed_Buf[i - 1] = Speed_Buf[i]; } Speed_Buf[FILTERING_TIMES - 1] = data; for (i = 0; i < FILTERING_TIMES; i++) { Sum_Data += Speed_Buf[i]; } Filter_Data = (s32)(Sum_Data / FILTERING_TIMES); return Filter_Data; } /************************************************************************** Function: Data sliding filtering Input : data Output : Filtered data º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨ Èë¿Ú²ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý **************************************************************************/ float Mean_Filter_Right(float data) { u8 i; float Sum_Data = 0; float Filter_Data; static float Speed_Buf[FILTERING_TIMES] = {0}; for (i = 1; i < FILTERING_TIMES; i++) { Speed_Buf[i - 1] = Speed_Buf[i]; } Speed_Buf[FILTERING_TIMES - 1] = data; for (i = 0; i < FILTERING_TIMES; i++) { Sum_Data += Speed_Buf[i]; } Filter_Data = (s32)(Sum_Data / FILTERING_TIMES); return Filter_Data; } /************************************************************************** Function: Lidar_Avoid Input : none Output : none º¯Êý¹¦ÄÜ£ºÀ×´ï±ÜÕÏģʽ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Lidar_Avoid(void) { int i = 0; u8 calculation_angle_cnt = 0; // ÓÃÓÚÅжÏ100¸öµãÖÐÐèÒª×ö±ÜÕϵĵã float angle_sum = 0; // ´ÖÂÔ¼ÆËãÕϰ­ÎïλÓÚ×ó»òÕßÓÒ u8 distance_count = 0; // ¾àÀëСÓÚijֵµÄ¼ÆÊý int distance = 350; // É趨±ÜÕϾàÀë,ĬÈÏÊÇ300 if (Car_Num == Akm_Car) distance = 400; // °¢¿ËÂü³µÉ趨ÊÇ400mm else if (Car_Num == Big_Tank_Car) distance = 500; // ´óÂÄ´ø³µÉ趨ÊÇ500mm for (i = 0; i < lap_count; i++) { if ((Dataprocess[i].angle > 310) || (Dataprocess[i].angle < 50)) { if ((0 < Dataprocess[i].distance) && (Dataprocess[i].distance < distance)) // ¾àÀëСÓÚ350mmÐèÒª±ÜÕÏ,Ö»ÐèÒª100¶È·¶Î§ÄÚµã { calculation_angle_cnt++; // ¼ÆËã¾àÀëСÓÚ±ÜÕϾàÀëµÄµã¸öÊý if (Dataprocess[i].angle < 50) angle_sum += Dataprocess[i].angle; else if (Dataprocess[i].angle > 310) angle_sum += (Dataprocess[i].angle - 360); // 310¶Èµ½50¶Èת»¯Îª-50¶Èµ½50¶È if (Dataprocess[i].distance < 200) // ¼Ç¼СÓÚ200mmµÄµãµÄ¼ÆÊý distance_count++; } } } if (calculation_angle_cnt < 8) // СÓÚ8µã²»ÐèÒª±ÜÕÏ£¬È¥³ýһЩÔëµã { if ((Move_X += 0.1) >= Aovid_Speed) // ±ÜÕϵÄËÙ¶ÈÉ趨Ϊ260£¬Öð½¥Ôö¼Óµ½260¿ÉÉÔ΢ƽ»¬Ò»Ð© Move_X = Aovid_Speed; Move_Z = 0; // ²»±ÜÕÏʱ²»ÐèҪתÍä } else // ÐèÒª±ÜÕÏ£¬¼òµ¥µØÅжÏÕϰ­Î﷽λ { if (Car_Num == Akm_Car) // °¢¿ËÂü³µÐÍÓжæ»ú£¬ÐèÒªÌØÊâ´¦Àí { if (distance_count > 8) // ¾àÀëСÓÚ±ÜÕ½¾àÀë Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ0.25 Move_X = Aovid_Speed * 0.5; if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ Move_Z = -Pi / 5; // ÿ´ÎתÍä½Ç¶ÈΪPI/5£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ else // Æ«×ó Move_Z = Pi / 5; } } else { if (distance_count > 8) // СÓÚ±ÜÕ½¾àÀëµÄʱºò Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ¶È0.15 Move_X = (Aovid_Speed * 0.5); if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ { if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ Move_Z = -1; else if (Car_Num == Small_Tank_Car) Move_Z = -1; else Move_Z = -1; } else // Æ«×ó { if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ Move_Z = 1; else if (Car_Num == Small_Tank_Car) Move_Z = 1; else Move_Z = 1; } } } } Move_Z = -Move_Z; } /************************************************************************** Function: Lidar_Follow Input : none Output : none º¯Êý¹¦ÄÜ£ºÀ×´ï¸úËæÄ£Ê½ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ float angle1 = 0; // ¸úËæµÄ½Ç¶È u16 mini_distance1; void Lidar_Follow(void) { static u16 cnt = 0; int i; int calculation_angle_cnt = 0; static float angle = 0; // ¸úËæµÄ½Ç¶È static float last_angle = 0; // u16 mini_distance = 65535; static u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿ // ÐèÒªÕÒ³ö¸úËæµÄÄǸöµãµÄ½Ç¶È for (i = 0; i < lap_count; i++) { if (100 < Dataprocess[i].distance && Dataprocess[i].distance < Follow_Distance) // 1200·¶Î§ÄÚ¾ÍÐèÒª¸úËæ { calculation_angle_cnt++; if (Dataprocess[i].distance < mini_distance) // ÕÒ³ö¾àÀë×îСµÄµã { mini_distance = Dataprocess[i].distance; angle = Dataprocess[i].angle; } } } if (angle > 180) angle -= 360; // 0--360¶Èת»»³É0--180£»-180--0£¨Ë³Ê±Õ룩 if (angle - last_angle > 10 || angle - last_angle < -10) // ×öÒ»¶¨Ïû¶¶£¬²¨¶¯´óÓÚ10¶ÈµÄÐèÒª×öÅÐ¶Ï { if (++data_count == 60) // Á¬Ðø60´Î²É¼¯µ½µÄÖµ(300msºó)ºÍÉϴεıȴóÓÚ10¶È£¬´Ëʱ²ÅÊÇÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } } else // ²¨¶¯Ð¡ÓÚ10¶ÈµÄ¿ÉÒÔÖ±½ÓÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } if (calculation_angle_cnt < 6) // ÔÚ¸úËæ·¶Î§ÄڵĵãÉÙÓÚ6¸ö { if (cnt < 40) // Á¬Ðø¼ÆÊý³¬40´ÎûÓÐÒª¸úËæµÄµã£¬´Ëʱ²ÅÊDz»ÓøúËæ cnt++; if (cnt >= 40) { Move_X = 0; // ËÙ¶ÈΪ0 Move_Z = 0; } } else { cnt = 0; if (Car_Num == Akm_Car) { if ((((angle > 15) && (angle < 180)) || ((angle > -180) && angle < -15)) && (mini_distance < 500)) // °¢¿¨Âü³µÐÍ´¦Àí³µÍ·²»¶ÔןúËæÎÏ൱ÓÚºó³µÒ»Ñù£¬Ò»´Î²»¶Ô×¼£¬ÄǺóÍËÔÙÀ´¶Ô×¼ { Move_X = -0.20; Move_Z = -Follow_Turn_PID(last_angle, 0); } else { Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm Move_Z = Follow_Turn_PID(last_angle, 0); } } else // ÆäÓà³µÐÍ { if ((angle > 50 || angle < -50) && (mini_distance > 400)) { Move_Z = -0.0298f * last_angle; // ½Ç¶È²î¾à¹ý´óÖ±½Ó¿ìËÙתÏò Move_X = 0; // ²îËÙС³µºÍÂÄ´øÐ¡³µ¿ÉÒÔʵÏÖÔ­µØ×ª¶¯ } else { Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm Move_Z = Follow_Turn_PID(last_angle, 0); // תÏòPID£¬³µÍ·ÓÀÔ¶¶ÔןúËæÎïÆ· } } } Move_Z = target_limit_float(Move_Z, -Pi / 6, Pi / 6); // ÏÞ·ù Move_X = target_limit_float(Move_X, -0.6, 0.6); } /************************************************************************** º¯Êý¹¦ÄÜ£ºÐ¡³µ×ßÖ±Ïßģʽ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Lidar_along_wall(void) { static u32 target_distance = 0; static int n = 0; u32 distance; u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿ for (int i = 0; i < lap_count; i++) { if (Dataprocess[i].angle > 75 && Dataprocess[i].angle < 77) { if (n == 0) { target_distance = Dataprocess[i].distance; // »ñÈ¡µÄµÚÒ»¸öµã×÷ΪĿ±ê¾àÀë n++; } if (Dataprocess[i].distance < target_distance + 100) //+100ÏÞÖÆ»ñÈ¡¾àÀëµÄ·¶Î§Öµ { distance = Dataprocess[i].distance; // »ñȡʵʱ¾àÀë data_count++; } } } // if(data_count <= 0) // Move_X = 0; // Move_X = forward_velocity; // ³õʼËÙ¶È Move_Z = -Along_Adjust_PID(distance, target_distance); if (Car_Num == Akm_Car) { Move_Z = target_limit_float(Move_Z, -Pi / 4, Pi / 4); // ÏÞ·ù } else if (Car_Num == Diff_Car) Move_Z = target_limit_float(Move_Z, -Pi / 5, Pi / 5); // ÏÞ·ù } /************************************************************************** Function: Car_Perimeter_Init Input : none Output : none º¯Êý¹¦ÄÜ£º¼ÆËãС³µ¸÷ÂÖ×ÓµÄÖܳ¤ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Car_Perimeter_Init(void) { if (Car_Num == Diff_Car || Car_Num == Akm_Car) { Perimeter = Diff_Car_Wheel_diameter * Pi; Wheelspacing = Diff_wheelspacing; } else if (Car_Num == Small_Tank_Car) { Perimeter = Small_Tank_WheelDiameter * Pi; Wheelspacing = Small_Tank_wheelspacing; } else { Perimeter = Big_Tank_WheelDiameter * Pi; Wheelspacing = Big_Tank_wheelspacing; } } /************************************************************************** Function: Ultrasonic_Follow Input : none Output : none º¯Êý¹¦ÄÜ£º³¬Éù²¨¸úËæÄ£Ê½ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Ultrasonic_Follow(void) // ³¬Éù²¨¸úËæ£¬Ö»Äܵ¥·½Ïò¸úËæ { Move_Z = 0; Read_Distane(); // ¶ÁÈ¡³¬Éù²¨µÄ¾àÀë if (Distance1 < 200) // ¾àÀëСÓÚ200mm£¬Í˺ó { if ((Move_X -= 3) < -210) Move_X = -210; // ¸øÒ»210ºóÍËËÙ¶È } else if (Distance1 > 270 && Distance1 < 750) // ¾àÀëÔÚ270µ½750Ö®¼äÊÇÐèÒª¸úËæÇ°½ø { if ((Move_X += 3) > 210) // ËÙ¶ÈÖð½¥Ôö¼Ó£¬¸øÇ°½øËÙ¶È Move_X = 210; } else { if (Move_X > 0) { if ((Move_X -= 20) < 0) // ËÙ¶ÈÖð½¥¼õµ½0 Move_X = 0; } else { if ((Move_X += 20) > 0) // ËÙ¶ÈÖð½¥¼õµ½0 Move_X = 0; } } } /************************************************************************** Function: Get angle Input : way£ºThe algorithm of getting angle 1£ºDMP 2£ºkalman 3£ºComplementary filtering Output : none º¯Êý¹¦ÄÜ£º»ñÈ¡½Ç¶È Èë¿Ú²ÎÊý£ºway£º»ñÈ¡½Ç¶ÈµÄËã·¨ 1£ºDMP 2£º¿¨¶ûÂü 3£º»¥²¹Â˲¨ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Angle(u8 way) { if (way == 1) // DMPµÄ¶ÁÈ¡ÔÚÊý¾Ý²É¼¯Öж϶ÁÈ¡£¬Ñϸñ×ñѭʱÐòÒªÇó { Read_DMP(); // ¶ÁÈ¡¼ÓËÙ¶È¡¢½ÇËÙ¶È¡¢Çã½Ç } else { Gyro_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_L); // ¶ÁÈ¡XÖáÍÓÂÝÒÇ Gyro_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_L); // ¶ÁÈ¡YÖáÍÓÂÝÒÇ Gyro_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_L); // ¶ÁÈ¡ZÖáÍÓÂÝÒÇ Accel_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_L); // ¶ÁÈ¡XÖá¼ÓËÙ¶È¼Æ Accel_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_L); // ¶ÁÈ¡XÖá¼ÓËÙ¶È¼Æ Accel_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_L); // ¶ÁÈ¡ZÖá¼ÓËÙ¶È¼Æ // if(Gyro_X>32768) Gyro_X-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» // if(Gyro_Y>32768) Gyro_Y-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» // if(Gyro_Z>32768) Gyro_Z-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_X>32768) Accel_X-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_Y>32768) Accel_Y-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_Z>32768) Accel_Z-=65536; //Êý¾ÝÀàÐÍת»» Accel_Angle_x = atan2(Accel_Y, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È Accel_Angle_y = atan2(Accel_X, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È Gyro_X = Gyro_X / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»»£¬Á¿³Ì¡À500¡ã/s¶ÔÓ¦ÁéÃô¶È65.5£¬¿É²éÊÖ²á Gyro_Y = Gyro_Y / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»» if (way == 2) { Roll = -Kalman_Filter_x(Accel_Angle_x, Gyro_X); // ¿¨¶ûÂüÂ˲¨ Pitch = -Kalman_Filter_y(Accel_Angle_y, Gyro_Y); } else if (way == 3) { Roll = -Complementary_Filter_x(Accel_Angle_x, Gyro_X); // »¥²¹Â˲¨ Pitch = -Complementary_Filter_y(Accel_Angle_y, Gyro_Y); } } } /************************************************************************** Function: The remote control command of model aircraft is processed Input : none Output : none º¯Êý¹¦ÄÜ£º¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Remote_Control(void) { // Data within 1 second after entering the model control mode will not be processed // ¶Ô½øÈ뺽ģ¿ØÖÆÄ£Ê½ºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí static u8 thrice = 200; int Threshold = 100; // limiter //ÏÞ·ù int LX, RY; // static float Target_LX,Target_LY,Target_RY,Target_RX; Remoter_Ch1 = target_limit_int(Remoter_Ch1, 1000, 2000); Remoter_Ch2 = target_limit_int(Remoter_Ch2, 1000, 2000); // Front and back direction of left rocker. Control forward and backward. // ×óÒ¡¸Ëǰºó·½Ïò¡£¿ØÖÆÇ°½øºóÍË¡£ LX = Remoter_Ch2 - 1500; // //Left joystick left and right. Control left and right movement. // //×óÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×óÓÒÒÆ¶¯¡£¡£ // LY=Remoter_Ch2-1500; // Right stick left and right. To control the rotation. // ÓÒÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×Ôת¡£ RY = Remoter_Ch1 - 1500; // if (LX > -Threshold && LX < Threshold) LX = 0; if (RY > -Threshold && RY < Threshold) RY = 0; // if(LX==0) Target_LX=Target_LX/1.2f; // if(LY==0) Target_LY=Target_LY/1.2f; // if(RY==0) Target_RY=Target_RY/1.2f; // //Throttle related //ÓÍÃÅÏà¹Ø // Remote_RCvelocity=RC_Velocity+RX; // if(Remote_RCvelocity<0)Remote_RCvelocity=0; // The remote control command of model aircraft is processed // ¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí Move_X = LX; Move_Z = -RY; Move_X = Move_X * 1.3; //*1.3ÊÇΪÁËÀ«´óËÙ¶È if (Car_Num == Akm_Car) Move_Z = Move_Z * (Pi / 8) / 350.0; else Move_Z = Move_Z * 2 * (Pi / 4) / 350.0; // Unit conversion, mm/s -> m/s // µ¥Î»×ª»»£¬mm/s -> m/s Move_X = Move_X / 1000; // ZÖáÊý¾Ýת»¯ #if _4WD if (Move_X < 0) Move_Z = -Move_Z; #endif // Data within 1 second after entering the model control mode will not be processed // ¶Ô½øÈ뺽ģ¿ØÖÆÄ£Ê½ºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí if (thrice > 0) Move_X = 0, Move_Z = 0, thrice--; // Control target value is obtained and kinematics analysis is performed // µÃµ½¿ØÖÆÄ¿±êÖµ£¬½øÐÐÔ˶¯Ñ§·ÖÎö // Get_Target_Encoder(Move_X,Move_Z); } 怎么把OpenMV与STM32串口通信实现二维码/APRILTag码识别及控制的代码加到上述代码中,现在openmv的二维码识别已做好,二维码采用的是Apriltag的36h11类型,id:0表示停止,id:1表示直行,id:2表示左转,id:3表示右转,现在需要再在STM32中修改或添加代码,使其能在接收到openmv识别到的指令后作出相应的动作

最新推荐

recommend-type

2022年网站美工个人年度工作总结(1).doc

2022年网站美工个人年度工作总结(1).doc
recommend-type

财务软件销售实习报告格式范文-实习报告格式(1).doc

财务软件销售实习报告格式范文-实习报告格式(1).doc
recommend-type

获取本机IP地址的程序源码分析

从给定文件信息中我们可以提取出的关键知识点是“取本机IP”的实现方法以及与之相关的编程技术和源代码。在当今的信息技术领域中,获取本机IP地址是一项基本技能,广泛应用于网络通信类的软件开发中,下面将详细介绍这一知识点。 首先,获取本机IP地址通常需要依赖于编程语言和操作系统的API。不同的操作系统提供了不同的方法来获取IP地址。在Windows操作系统中,可以通过调用Windows API中的GetAdaptersInfo()或GetAdaptersAddresses()函数来获取网络适配器信息,进而得到IP地址。在类Unix操作系统中,可以通过读取/proc/net或是使用系统命令ifconfig、ip等来获取网络接口信息。 在程序设计过程中,获取本机IP地址的源程序通常会用到网络编程的知识,比如套接字编程(Socket Programming)。网络编程允许程序之间进行通信,套接字则是在网络通信过程中用于发送和接收数据的接口。在许多高级语言中,如Python、Java、C#等,都提供了内置的网络库和类来简化网络编程的工作。 在网络通信类中,IP地址是区分不同网络节点的重要标识,它是由IP协议规定的,用于在网络中唯一标识一个网络接口。IP地址可以是IPv4,也可以是较新的IPv6。IPv4地址由32位二进制数表示,通常分为四部分,每部分由8位构成,并以点分隔,如192.168.1.1。IPv6地址则由128位二进制数表示,其表示方法与IPv4有所不同,以冒号分隔的8组16进制数表示,如2001:0db8:85a3:0000:0000:8a2e:0370:7334。 当编写源代码以获取本机IP地址时,通常涉及到以下几个步骤: 1. 选择合适的编程语言和相关库。 2. 根据目标操作系统的API或系统命令获取网络接口信息。 3. 分析网络接口信息,提取出IP地址。 4. 将提取的IP地址转换成适合程序内部使用的格式。 5. 在程序中提供相应功能,如显示IP地址或用于网络通信。 例如,在Python中,可以使用内置的socket库来获取本机IP地址。一个简单的示例代码如下: ```python import socket # 获取主机名 hostname = socket.gethostname() # 获取本机IP local_ip = socket.gethostbyname(hostname) print("本机IP地址是:", local_ip) ``` 在实际应用中,获取本机IP地址通常是为了实现网络通信功能,例如建立客户端与服务器的连接,或者是在开发涉及到IP地址的其他功能时使用。 关于文件名称“getIP”,这是一个自解释的文件名,明显表示该文件或程序的作用是获取本机的IP地址。从标签“控件 源码 网络通信类 资源”中我们可以看出,这个文件很可能是一个可以嵌入其他软件中的代码片段,用以实现网络通信功能的一部分,具有较高的灵活性和重用性。 综上所述,获取本机IP地址是一个涉及到操作系统API、网络编程以及特定编程语言知识的技能。它不仅需要对编程语言提供的库函数有深入的理解,还要对操作系统的网络配置和IP协议有足够的认识。通过阅读和分析相关的源代码,如“getIP”,可以加深对网络编程实现细节的理解,并能够在实际开发中快速地应用这一技术。
recommend-type

【权威指南】:Win11笔记本上RTX4080驱动的安装与调试秘籍

# 1. Win11笔记本与RTX4080显卡介绍 随着技术的不断发展,笔记本电脑已不再是仅仅满足办公需求的移动设备,而是逐渐成为可以胜任复杂图形处理和游戏任务的强大工具。微软推出的Windows 11操作系统,以其全新的用户界面和改进的系统性能,为用户提供了更为流畅的操作体验。而在硬件方面,NVIDIA的RTX 4080显卡以其卓越的图形处理能力和AI支持,为笔记本电脑带来了前所未有的性能提升。本章节将带您初步了解Windows 11笔记本与NVIDIA RTX 4080显卡的基础信息,为进一步深入探讨它们的优化和调试技巧奠定基础。 ## 1.1 Win11笔记本的优势 Windows
recommend-type

windows环境举例

<think>好的,用户现在想在Windows环境下创建C函数库,需要具体的方法和示例代码。我需要结合之前的回答和用户提供的引用内容来给出合适的指导。 首先回顾之前的回答,用户已经了解了在Linux下创建静态库和动态库的方法,现在需要切换到Windows环境。根据引用[2],Windows下的C标准库和动态链接库的处理与Linux不同,比如使用dlfcn.h在Linux,而Windows可能需要其他方式。另外,引用[1]提到了在Windows下配置gcc环境(MinGW-w64),这可能是一个关键点,因为用户可能需要使用MinGW来编译库。 用户提供的引用[3]提到了使用MSVC编译器,这
recommend-type

QQ自动发送/回复系统源代码开放

根据提供的文件信息,我们可以了解到以下几点关键的知识点: ### 标题:“qqhelp” 1. **项目类型**: 标题“qqhelp”暗示这是一个与QQ相关的帮助工具或项目。QQ是中国流行的即时通讯软件,因此这个标题表明项目可能提供了对QQ客户端功能的辅助或扩展。 2. **用途**: “help”表明此项目的主要目的是提供帮助或解决问题。由于它提到了QQ,并且涉及“autosend/reply”功能,我们可以推测该项目可能用于自动化发送消息回复,或提供某种形式的自动回复机制。 ### 描述:“I put it to my web, but nobody sendmessage to got the source, now I public it. it supply qq,ticq autosend/reply ,full sourcecode use it as you like” 1. **发布情况**: 描述提到该项目原先被放置在某人的网站上,并且没有收到请求源代码的消息。这可能意味着项目不够知名或者需求不高。现在作者决定公开发布,这可能是因为希望项目能够被更多人了解和使用,或是出于开源共享的精神。 2. **功能特性**: 提到的“autosend/reply”表明该项目能够实现自动发送和回复消息。这种功能对于需要进行批量或定时消息沟通的应用场景非常有用,例如客户服务、自动化的营销通知等。 3. **代码可用性**: 作者指出提供了“full sourcecode”,意味着源代码完全开放,用户可以自由使用,无论是查看、学习还是修改,用户都有很大的灵活性。这对于希望学习编程或者有特定需求的开发者来说是一个很大的优势。 ### 标签:“综合系统类” 1. **项目分类**: 标签“综合系统类”表明这个项目可能是一个多功能的集成系统,它可能不仅限于QQ相关的功能,还可能包含了其他类型的综合服务或特性。 2. **技术范畴**: 这个标签可能表明该项目的技术实现比较全面,可能涉及到了多个技术栈或者系统集成的知识点,例如消息处理、网络编程、自动化处理等。 ### 压缩包子文件的文件名称列表: 1. **Unit1.dfm**: 这是一个Delphi或Object Pascal语言的窗体定义文件,用于定义应用程序中的用户界面布局。DFM文件通常用于存储组件的属性和位置信息,使得开发者可以快速地进行用户界面的设计和调整。 2. **qqhelp.dpr**: DPR是Delphi项目文件的扩展名,包含了Delphi项目的核心设置,如程序入口、使用的单元(Units)等。这个文件是编译和构建Delphi项目的起点,它能够帮助开发者了解项目的组织结构和编译指令。 3. **Unit1.pas**: PAS是Delphi或Object Pascal语言的源代码文件。这个文件可能包含了与QQ帮助工具相关的核心逻辑代码,例如处理自动发送和回复消息的算法等。 4. **readme.txt**: 这是一个常见的文本文件,包含项目的基本说明和使用指导,帮助用户了解如何获取、安装、运行和定制该项目。README文件通常是用户与项目首次交互时首先阅读的文件,因此它对于一个开源项目的用户友好度有着重要影响。 通过以上分析,我们可以看出“qqhelp”项目是一个针对QQ通讯工具的自动化消息发送与回复的辅助工具。项目包含完整的源代码,用户可以根据自己的需要进行查看、修改和使用。它可能包含Delphi语言编写的窗体界面和后端逻辑代码,具有一定的综合系统特性。项目作者出于某种原因将其开源,希望能够得到更广泛的使用和反馈。
recommend-type

【7步打造Win11深度学习利器】:Tensorflow-GPU与RTX4080终极优化指南

# 1. 深度学习与Windows 11的融合 在人工智能时代,深度学习已渗透到生活的方方面面,而Windows 11作为最新一代的操作系统,为深度学习提供了一个高效的工作平台。本章节将探讨深度学习技术如何与Windows 11系统融合,以及这一
recommend-type

ue画笔画画蓝图版

<think>我们正在讨论的是在UnrealEngine中使用蓝图实现画笔绘画功能。根据用户需求,重点在于通过蓝图系统实现类似毛笔的绘画效果。结合之前的回答和引用内容,我们将详细展开实现方法。核心思路:通过捕捉输入轨迹,动态生成笔触网格,并应用材质模拟墨迹效果。###详细实现步骤####1.创建绘画蓝图创建一个名为`BP_PaintBrush`的Actor蓝图:-**根组件**:SceneComponent-**关键组件**:-`SplineComponent`:用于存储绘画路径点-`InstancedStaticMeshComponent`:高效渲染重复笔触段(替代单个SplineMesh组
recommend-type

VB.NET图表曲线组件实现多种图表绘制

在深入讨论所给文件信息中的知识点之前,我们首先需要明确这些信息所代表的内容。标题指出我们所讨论的是一款在VB.NET环境中使用的“三维图表曲线组件”。从描述中我们可以了解到该组件的功能特性,即它能够绘制包括柱状图、线条曲线图和饼图在内的多种类型图表,并且支持图例的展示。此外,组件的色彩使用比较鲜艳,它不仅适用于标准的Windows Forms应用程序,还能够在ASP.NET环境中使用。而“压缩包子文件的文件名称列表”提供的信息则指向了可能包含该组件示例代码或说明文档的文件名,例如“PSC_ReadMe_4556_10.txt”可能是一个说明文档,而“GraphingV3Testing”和“Graphing.V3”则可能是一些测试文件或组件的实际使用案例。 下面详细说明标题和描述中提到的知识点: 1. VB.NET环境中的图表组件开发: 在VB.NET中开发图表组件需要开发者掌握.NET框架的相关知识,包括但不限于Windows Forms应用程序的开发。VB.NET作为.NET框架的一种语言实现,它继承了.NET框架的面向对象特性和丰富的类库支持。图表组件作为.NET类库的一部分,开发者可以通过继承相关类、使用系统提供的绘图接口来设计和实现图形用户界面(GUI)中用于显示图表的部分。 2. 图表的类型和用途: - 柱状图:主要用于比较各类别数据的数量大小,通过不同长度的柱子来直观显示数据间的差异。 - 线条曲线图:适用于展示数据随时间或顺序变化的趋势,比如股票价格走势、温度变化等。 - 饼图:常用于展示各部分占整体的比例关系,可以帮助用户直观地了解数据的组成结构。 3. 图例的使用和意义: 图例在图表中用来说明不同颜色或样式所代表的数据类别或系列。它们帮助用户更好地理解图表中的信息,是可视化界面中重要的辅助元素。 4. ASP.NET中的图表应用: ASP.NET是微软推出的一种用于构建动态网页的框架,它基于.NET平台运行。在ASP.NET中使用图表组件意味着可以创建动态的图表,这些图表可以根据Web应用程序中实时的数据变化进行更新。比如,一个电子商务网站可能会利用图表组件来动态显示产品销售排行或用户访问统计信息。 5. 色彩运用: 在设计图表组件时,色彩的运用非常关键。色彩鲜艳不仅能够吸引用户注意,还能够帮助用户区分不同的数据系列。正确的色彩搭配还可以提高信息的可读性和美观性。 在技术实现层面,开发者可能需要了解如何在VB.NET中使用GDI+(Graphics Device Interface)进行图形绘制,掌握基本的绘图技术(如画线、填充、颜色混合等),并且熟悉.NET提供的控件(如Panel, Control等)来承载和显示这些图表。 由于提供的文件名列表中包含有"Testing"和".txt"等元素,我们可以推测该压缩包内可能还包含了与图表组件相关的示例程序和使用说明,这对于学习如何使用该组件将十分有用。例如,“GraphingV3Testing”可能是一个测试项目,用于在真实的应用场景中检验该图表组件的功能和性能;“PSC_ReadMe_4556_10.txt”可能是一个详细的用户手册或安装说明,帮助用户了解如何安装、配置和使用该组件。 总结而言,了解并掌握在VB.NET环境下开发和使用三维图表曲线组件的知识点,对从事.NET开发的程序员来说,不仅可以增强他们在数据可视化方面的技能,还可以提高他们构建复杂界面和动态交互式应用的能力。
recommend-type

【MultiExtractor_Pro实战演练】:8个复杂场景的高效数据提取解决方案

# 摘要 本文介绍了MultiExtractor_Pro工具的概述、基本使用技巧、实战演练、高级功能应用以及案例研究与展望。首先,详细说明了MultiExtractor_Pro的安装过程和用户界面布局,阐述了核心功能组件及其操作方法。接着,讲述了配置提取模板、设置提取任务以及实时数据提取与预览技巧。在实