瞬驰(Dash)D1开发手册--超声波传感器v2版

最新推荐文章于 2023-06-10 20:29:04 发布

EAIBOT

最新推荐文章于 2023-06-10 20:29:04 发布

阅读量1.8k

点赞数

分类专栏： EAI 文章标签：超声波传感器

本文链接：https://blog.csdn.net/EAIBOT/article/details/51455571

版权

EAI 专栏收录该内容

8 篇文章 3 订阅

订阅专栏

在瞬驰(Dash)D1前部安装3个超声波传感器，后部安装1个超声波传感器。

在arduino mega2560固件中添加如下代码：

int TrigPin2 = 42;
int EchoPin2 = 43;
int TrigPin3 = 44;
int EchoPin3 = 45;
int TrigPin4 = 46;
int EchoPin4 = 47;
int TrigPin5 = 48;
int EchoPin5 = 49;

int myPing(int TrigPin, int EchoPin) {  
    digitalWrite(TrigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(TrigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(TrigPin, LOW);
   return (int)(pulseIn(EchoPin, HIGH)/58.0);
}

在命令处理中添加如下代码：

    case PING:
      Serial.print(myPing(TrigPin2, EchoPin2));
      Serial.print(" ");
      Serial.print(myPing(TrigPin3, EchoPin3));
      Serial.print(" ");
      Serial.print(myPing(TrigPin4, EchoPin4));
      Serial.print(" ");
      Serial.println(myPing(TrigPin5, EchoPin5));
      break;

测试代码片段：

 // test ultrasonic
 #if 0
  delay(500);
    Serial.print("ultrasonic: ");
    Serial.print(myPing(TrigPin2 , EchoPin2 ));
    Serial.print(", ");
   Serial.print(myPing(TrigPin3 , EchoPin3));
   Serial.print(", ");
   Serial.print(myPing(TrigPin4 , EchoPin4 ));
   Serial.print(", ");
   Serial.println(myPing(TrigPin5 , EchoPin5 ));
#endif

ros_arduino_bridge端

在ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py中，把ping()函数修改为如下内容：

    def ping(self):
        #return int(self.execute('p %d' %id));
        values = self.execute_array('p')
        if len(values) != 4:
            print "Encoder count was not 4"
            raise SerialException
            return None
        else:
            return values

在ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py的poll()中读取超声波传感器的值



            try:
                r1,r2,r3,r4 = self.arduino.ping()
            except:
                rospy.logerr("ping exception")
                return
            rospy.loginfo("ranger: " + str(r1)+","+str(r2)+","+str(r3)+","+str(r4))