在瞬驰(Dash)D1前部安装3个超声波传感器。
arduino端
在arduino固件中添加如下代码:
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 (long)(pulseIn(EchoPin, HIGH) / 58.0/100.0);
return (int)(pulseIn(EchoPin, HIGH)/58.0);
}
int last_f1=400;
int last_f2=400;
int last_f3=400;
int frontPing(){
int f1= myPing(TrigPin2 , EchoPin2 );
int f2= myPing(TrigPin5 , EchoPin5 );
int f3= myPing(TrigPin4 , EchoPin4);
if(f1==0){
f1=last_f1;
}else if ((f1>=400)||(f1<0)){
f1=400;
}
if(f2==0){
f2=last_f1;
}else if ((f2>=400)||(f2<0)){
f2=400;
}
if(f3==0){
f3=last_f1;
}else if ((f3>=400)||(f3<0)){
f3=400;
}
int tmp=min(f1,f2);
int tmp1=min(tmp, f3);
if(f1>0){
last_f1=f1;
}
if(f2>0){
last_f2=f2;
}
if(f3>0){
last_f3=f3;
}
return tmp1;
}
在命令处理中添加如下代码:
case PING:
Serial.println(frontPing());
break;
测试代码片段:
#if 0
delay(3000);
Serial.print("front: ");
Serial.println(frontPing());
Serial.print("2: ");
Serial.println(last_f1);
Serial.print("3: ");
Serial.println(last_f2);
Serial.print("4: ");
Serial.println(last_f3);
#endif
ros_arduino_bridge端
在ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py中,把ping()函数修改为如下内容:
def ping(self):
‘‘‘ The srf05/Ping command queries an SRF05/Ping sonar sensor
connected to the General Purpose I/O line pinId for a distance,
and returns the range in cm. Sonar distance resolution is integer based.
‘‘‘
return self.execute(‘p‘);
在ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py修改如下三处:
1)在init()中定义变量
# ranger
self.ranger_front=400
2)在poll()中读取超声波传感器的值
try:
self.ranger_front= self.arduino.ping()
except:
rospy.logerr("ping exception count: ")
return
rospy.loginfo("ranger_front:" + str(self.ranger_front))
3)速度的调节
if self.ranger_front < 25 and x>0:
x=0.0
elif self.ranger_front < 50 and x>=0.15:
x=0.15
elif self.ranger_front < 80 and x>=0.3:
x=0.3
时间: 2024-10-09 05:11:58