Tacit Project com hc-sr04

[Michel Brito]

Olá, sou estudante de eletrônica (curso técnico) e estou fazendo meu TCC, eu escolhi este projeto para desenvolver e aperfeiçoar (http://grathio.com/2011/08/meet-the-tacit-project-its-sonar-for-the-blind/) é um projeto não muito difícil, porém estou tendo problemas com a programação, no projeto é usado um sensor ultrassônico de 3 pinos, para baratear o projeto eu estou utilizando um sensor ultrassônico hc-sr04, não manjo mt de C++, gostaria q algum pudesse me ajudar com isso,  já que a dificuldade(para mim) é desenvolver a programação para o hc-sr04.

Obrigado 

•  

Código abaixo

eu achei em um fórum inglês, este já esta adaptado para o hc-sr04, porém quando carrego p o arduino, os servos motores ficam em uma posição e independentemente da distancia q estou do hc-sr04 eles não se mexem.

#define _DEBUG_MODE 1
#define _DEBUG_SENSOR 1 

#define LEFT_SERVO_PIN 4
#define RIGHT_SERVO_PIN 7

#include <NewPing.h>
#include <Servo.h>

#define SENSOR_NUM     2 
#define MAX_DISTANCE 350 
#define PING_INTERVAL 50 

unsigned long pingTimer[SENSOR_NUM]; 
unsigned int cm[SENSOR_NUM];         
byte currentSensor = 0;        

NewPing sonar[SENSOR_NUM] = {    
  NewPing(8, 9, MAX_DISTANCE),
  NewPing(10, 11, MAX_DISTANCE)
};

Servo ServoList[SENSOR_NUM];
const int ServoPins[SENSOR_NUM] = {LEFT_SERVO_PIN,RIGHT_SERVO_PIN};      
const int ServoMaxAngle[SENSOR_NUM] = {90,90}; 
const int ServoMinAngle[SENSOR_NUM] = {0,180};
const int SensorClose = 10;                   
const int SensorFar = 14000;                   
const int ReadingsPerSensor = 3;              
const int TimePerDegree = 30;                  
const int MinimumTurnDistance = 3;           

int sensorReadings[SENSOR_NUM][ReadingsPerSensor];   
int calculatedSensorReadings[SENSOR_NUM];            
int latestReading = 0;                             
int servoLocations[SENSOR_NUM];                 

void setup() {
  Serial.begin(9600);
  Serial.println("Tacit Glove Project");

  pingTimer[0] = millis() + 75;           
  for (uint8_t i = 1; i < SENSOR_NUM; i++) {
    pingTimer = pingTimer[i - 1] + PING_INTERVAL;  
  }
  for (uint8_t i = 0; i < SENSOR_NUM; i++) {  
    ServoList.attach(ServoPins);  
    delay(10);
    ServoList.write(ServoMaxAngle);
    delay(500);
    ServoList.write(ServoMinAngle);
    delay(500);
    ServoList.write(90);
  }
}

void loop() {
  int i, j, oldLocation;
  unsigned long delayTime;
  for (i = 0; i < SENSOR_NUM; i++){
    sensorReadings[latestReading] = getDistance(i);
    calculatedSensorReadings = calculateNewDistace(i);
    oldLocation = servoLocations;
    servoLocations = map(calculatedSensorReadings, 0, 100, ServoMinAngle, ServoMaxAngle);

    if (latestReading >= ReadingsPerSensor-1){                         
      if (abs(servoLocations-oldLocation) >= MinimumTurnDistance){   
        ServoList.attach(ServoPins);
        delay(10);
        ServoList.write(servoLocations);
        delayTime = (TimePerDegree * (abs(servoLocations-oldLocation))+20);     
        if (abs(delayTime)>500){ 
            delayTime=500;         
        }
        delay(delayTime);
        ServoList.detach();
      } 
      else {                                         
        ServoList.attach(ServoPins);            
        delay(10);
        ServoList.write(oldLocation);
        delay(50);         
        ServoList.detach();   
        servoLocations=oldLocation;
      }
    }
    delay(20);
  }

  latestReading++;
  if (latestReading >= ReadingsPerSensor){  
    latestReading = ReadingsPerSensor-1;
    for (i = 0; i < SENSOR_NUM; i++){
      for (j=0; j < ReadingsPerSensor-1; j++){
        sensorReadings[j] = sensorReadings[j+1];
      }
    }
  }

}

void echoCheck() { 
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

int calculateNewDistace(int sensorNumber){
  int output = SensorFar;                     

  float weightingFactor = 0.5;                
  float flickerFactor = 30;                    /

  if (latestReading >= ReadingsPerSensor-1) { 
    int total = 0;                            
    float currentWeight = 1;                  
    float percentagePossible = 0;
    boolean flickered = false;
    for (int i=ReadingsPerSensor-1; i >=0; i--){  
      flickered = false;
      if (i==ReadingsPerSensor-1){
        if ((abs(sensorReadings[sensorNumber])-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] * currentWeight);
        percentagePossible += currentWeight;
        currentWeight *= weightingFactor;
      }
    }
    output = total / percentagePossible;
  }
  return output;
}

int getDistance(int sensorNumber){
  long duration;   
  int out;         

  duration = sonar[sensorNumber].ping();

  // Trim the data into minimums and maximums and map it to the 0-100 output range.
  duration = constrain(duration, SensorClose, SensorFar);
  out = map(duration,  SensorClose, SensorFar, 0, 100);
#if _DEBUG_MODE
  if (sensorNumber == _DEBUG_SENSOR) {  
    Serial.print("Time required by pulse to come back: ");
    Serial.println(duration);
    Serial.print("What will be send back: ");
    Serial.println(out);
  }
#endif
  
 
  if (duration != 10) {
    return out;
  } else {
    return map(SensorFar, SensorClose, SensorFar, 0, 100);
  }
}

 

Editado Setembro 10, 2017 por Michel Brito