Не работают двигатели при работе с Seeed Ultrasonic Sensor.

Ребята, подскажите почему не работает. Если хоть раз вызывается функция ping(), то двигатели не крутятся, а если переменной dst присвоить значение удовлетворяющее условию, то всё работает, даже если ping() вызовем в setup(){}, всёравно двигадели не работают, а если после этого в loop(){} вставить что-то типа Serial.println(ping()); На экран выводит расстояние, но двигатели не крутятся, почему? Вот сам скетч:

Код (Text):

#include <AFMotor.h>
AF_DCMotor m1(1);
AF_DCMotor m2(2);
const int pingPin = 7;
const int danger = 20;
long dst;
long duration;
void setup(){
  Serial.begin(9600);
}
void loop(){
    dst = ping();
  //Serial.println(ping());
  if((dst <= danger) && (dst != 0)){
  m1.run(BACKWARD);
  m2.run(BACKWARD);
  m1.setSpeed(200);
  m2.setSpeed(200);
  delay(3000);
  m1.run(RELEASE);
  m2.run(RELEASE);
    }
  else {
  m1.run(BACKWARD);
  m2.run(FORWARD);
  m1.setSpeed(200);
  m2.setSpeed(200);
  delay(3000);
  m1.run(RELEASE);
  m2.run(RELEASE);
 
    }
}
long ping(){
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration= pulseIn(pingPin, HIGH);
  return duration / 29 / 2;
 
}

Платы: Freeduino 2009 (http://freeduino.ru/arduino/freeduino2009.html)
Motor Shield v3 (http://freeduino.ru/arduino/mshield.html)
Seeed Ultrasonic Sensor (http://www.seeedstudio.com/wiki/Ultra_Sonic_range_measurement_module)

 

А это "Serial.println(ping());" что говорит?

 

1. В коде вроде ничего криминального не видно, разве что условие нужно исправить - сначала проверяем на 0, а затем сравниваем расстояние. Иначе получается, что в else мы попадем не только когда расстояние мало, но и когда прочитался 0, т.е. в случае таймаута.
2. Напихайте повсюду отладочный вывод - он поможет лучше понять, что происходит в программе.