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

在瞬驰(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))