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

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