更新arduino代码,走迷宫避障参数

src/CMakeLists.txt

@@ -1 +1 @@
-/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
+/opt/ros/kinetic/catkin/cmake/toplevel.cmake

src/infrared_maze/cfg/param.yaml

@@ -15,7 +15,7 @@ dist_service: /mobilebase_arduino/getInfraredDistance
 linear_x: 0.17
 linear_y: 0.15
 angular_speed: 0.5
-warn_dist: 0.12
+warn_dist: 0.15
 tolerance_dist: 0.05
 odom_frame: /odom_combined
 base_frame: /base_footprint

src/infrared_maze/src/infrared_maze.cpp

@@ -174,7 +174,7 @@ void goForward()
     distSrvClient.call(distSrv); //通过服务调用来获取测距值
     front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
     ROS_INFO("Front dist:%f", front_dist);
-    while((dist<check_dist)&&(front_dist>tolerance_dist*3.0)&&(ros::ok()))
+    while((dist<check_dist)&&(front_dist>tolerance_dist*4.0)&&(ros::ok()))
     {
         ROS_INFO("Go forward, dist:%f", dist);
         publishTwistCmd(linear_x_speed, 0, 0);
@@ -337,7 +337,7 @@ void horizMoveMiddle(float tolerance)
  *   会错.这里的走迷宫策略是左转优先,中间是直行,最后是右转.
  **************************************************************************/
 void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
-        float tolerance)
+        float tolerance, float warn_distance)
 {
     static int flag = 0;
 
@@ -387,7 +387,7 @@ void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
     right_dist = msg->right_dist;
 
     //根据三个传感器反馈的测距值开始移动
-    checkInfraredDist(front_dist, left_dist, right_dist, tolerance_dist);
+    checkInfraredDist(front_dist, left_dist, right_dist, tolerance_dist, warn_dist);
     yawUpdate(start_yaw_data); //移动完成后开始更新yaw值
 }
 

src/robot_navigation/maps/mymap.yaml

@@ -0,0 +1,7 @@
+image: /home/corvin/blackTornadoRobot/src/robot_navigation/maps/mymap.pgm
+resolution: 0.020000
+origin: [-15.880000, -5.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -49,13 +49,13 @@ AWheel_Kd: 34
 AWheel_Ki: 0
 AWheel_Ko: 50
 
-BWheel_Kp: 15
-BWheel_Kd: 15
+BWheel_Kp: 22
+BWheel_Kd: 24
 BWheel_Ki: 0
 BWheel_Ko: 50
 
-CWheel_Kp: 18
-CWheel_Kd: 15
+CWheel_Kp: 22
+CWheel_Kd: 30
 CWheel_Ki: 0
 CWheel_Ko: 50
 

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -79,7 +79,7 @@ class Arduino:
 
     def i2c_connect(self):
         try:
-            rospy.loginfo("Connecting to Arduino on IIC addr:"+self.i2c_slave_addr)
+            rospy.loginfo("Connecting to Arduino on IIC addr:"+str(self.i2c_slave_addr))
             test = self.i2c_get_baud()
             if test != self.baudrate:
                 time.sleep(1)