Arduino.ru

Здравствуйте помогите пожалуйста.

Имеется вот такой код для моего проекта. Проблема в том что, код написан для датчиков паралакс, а у меня используются hc-sr04, в котором 4 вывода.  И уменя этих датчиков в проекте используется два. Я попробывал через newping, вот не знаю правильно ли прописал пины датчиков. Библиотеки все ставил.

#include <NewPing.h>
#include <Servo.h>
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

const int MaxSensors=2;
unsigned long pingTimer[MaxSensors];
unsigned int cm[MaxSensors];        
uint8_t currentSensor = 0;

const int ServoPins[MaxSensors]={7, 8};
 NewPing RangingPins[MaxSensors]={
  NewPing(2, 3, MAX_DISTANCE),
  NewPing(4, 5, MAX_DISTANCE)
};

const int ReadingsPerSensor = 5;              // количество сохраняемых чтений для определегия положения
const int TimePerDegree = 9;                  // ms на 1 градус поворота сервопривода для предотвращения влияния сенсора
const int MinimumTurnDistance = 3;            // минимальный угол поворота сервопривода для избежания дребезга
// Переменные
Servo ServoList[MaxSensors];                         // массив сервопривода
int sensorReadings[MaxSensors][ReadingsPerSensor];   // сохранение предыдущего состояния сенсора
int calculatedSenorReadings[MaxSensors];             // расчётное расстояние для каждого сенсора
int latestReading = 0;                               // текущее состояние после считывания
int servoLocations[MaxSensors];                      // текущее состояние каждого сенсора
int SenorClose = 500;                                // минимальное значение времени на одно перемещение - полсекунды
int SensorFar = 14000;                               // максимальное время на перемещение - 14 секунд
int ServoClose[MaxSensors] = {0, 160};               // угол поворота сервопривода при приближении
int ServoFar[MaxSensors] = {70,110};                 // угол поворота при отдалении
void setup() {
  //Serial.begin(115200);                             // открываем порт для теста
  //Serial.println("Begin...");
  // инициализация сервоприводов
  for (int i = 0; i < MaxSensors; i++){
     ServoList[i].attach(ServoPins[i]);
     delay(10);
     ServoList[i].write(ServoClose[i]);
     delay(500);
     ServoList[i].write(ServoFar[i]);
     delay(500);
     ServoList[i].detach();
   }
   delay(100);
}
void loop(){
  int i, j, oldLocation;
  unsigned long delayTime;
  // цикл по каждому диапазону сенсора
  for (i = 0; i < MaxSensors; i++){
    // Get the current sensor's range.
    sensorReadings[i][latestReading] = getDistance(i);
    // Figure out an averaged/smoothed readings based on this and past data.
    calculatedSenorReadings[i] = calculateNewDistace(i);
    // Set the servo to the correct angle.
    oldLocation = servoLocations[i];
    servoLocations[i] = map(calculatedSenorReadings[i], 0, 100, ServoClose[i], ServoFar[i]);
    if (latestReading >= ReadingsPerSensor-1){                          // Don't do anything until we have enough data to trend.
      if (abs(servoLocations[i]-oldLocation) >= MinimumTurnDistance){   // Only try to turn it if we have somewhere to go.
               ServoList[i].attach(ServoPins[i]);
               delay(10);
               ServoList[i].write(servoLocations[i]);
               delayTime = (TimePerDegree * (abs(servoLocations[i]-oldLocation))+20);      // Set a delay for the next reading so motor noise doesn't interfere with senor readings.
               if (abs(delayTime)>500){ // If it can't do it in this amount of time       // It's based on how far it has to turn to keep the delay to a minimum, response time at a maximum.
                    delayTime=500;         // we'll get it next time. Keep it responsive.
               }
               delay(delayTime);
               ServoList[i].detach();
         } else {                                          // Otherwise if the reading hasn't changed enough write the old value to
             ServoList[i].attach(ServoPins[i]);            // the servo so that it will hold in place if it's applying pressure.
               delay(10);
               ServoList[i].write(oldLocation);
               delay(50);        
               ServoList[i].detach();  
             servoLocations[i]=oldLocation;
         }
    }
    delay(20); // Added to fix left sensor misbehavior reported by Rob.
  }
  latestReading++; // Increment the reading counter so we know where we're at.
  if (latestReading >= ReadingsPerSensor){  // Make sure we don't record more readings than we have space to hold.
    latestReading = ReadingsPerSensor-1;
    // Pop the oldest reading off the list.
    for (i = 0; i < MaxSensors; i++){
      for (j=0; j < ReadingsPerSensor-1; j++){
        sensorReadings[i][j] = sensorReadings[i][j+1];
      }
    }
  }
}
// function: calculateNewDistace(sensorNumber: Which sensor's data to process): Calculated distance in 0-100 range.
// Apply some averaging and smoothing to the recorded distance readings
// to take care of noisy data.
int calculateNewDistace(int sensorNumber){
  int output = SensorFar;                      // Default value is the furthest distance.

  float weightingFactor = 0.5;                 // How fast the reading's importance tapers off in time. (1= no taper, 0 = divide by zero error.)
  float flickerFactor = 30;                    // When the change is greater than this, ignore it unless its two in a row. (It's probably noise.)
  if (latestReading >= ReadingsPerSensor-1) {  // Only do this if we have a full set of readings to sample.
    int total = 0;                             // Average them with a weighting.
    float currentWeight = 1;                   // New readings count more than older readings.
    float percentagePossible = 0;
    boolean flickered = false;
    for (int i=ReadingsPerSensor-1; i >=0 ;i--){   // Check for flicker (This reduces jitter with something right on the threshold.)
      flickered = false;
      if (i==ReadingsPerSensor-1){
        if ((abs(sensorReadings[sensorNumber][i])-abs(sensorReadings[sensorNumber][i-1]) > flickerFactor) &&
           (abs(sensorReadings[sensorNumber][i-1])-abs(sensorReadings[sensorNumber][i-2]) > flickerFactor)){
          flickered = true;
        }
      }
      if (flickered==false){
        total += (sensorReadings[sensorNumber][i] * currentWeight);
        percentagePossible += currentWeight;
        currentWeight *= weightingFactor;
      }
    }
    output = total / percentagePossible;
  }
  return output;
}
// function: getDistance

int getDistance(int sensorNumber)
{
  unsigned int duration;   // How long it takes a sonic pulse to reflect back.
  int out;         // The value we send back from the function
  delayMicroseconds(5);
  duration = RangingPins[sensorNumber].ping();
  // Trim the data into minimums and maximums and map it to the 0-100 output range.
  duration = constrain(duration, SenorClose, SensorFar);
  out = map(duration,  SenorClose, SensorFar, 0, 100);
  return out;
}