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

来源：互联网发布：淘宝羊毛衫编辑：程序博客网时间：2024/06/05 14:50

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

0 0