teste.ino

#include <SoftwareServo.h>

//constantes

const int MAX_SERVOS  = 2;
const int servoInput[MAX_SERVOS] = { 3,2 };
const int sensorPerto = 500;
const int sensorLonge = 4500;
const int maxLeituraPorSensor = 5;
const int servoPosLonge[MAX_SERVOS] = {0,160 };
const int servoPosPerto[MAX_SERVOS] = {70, 110};

const int sensorInputTrigger[MAX_SERVOS] = {8,2};
const int sensorInputEcho[MAX_SERVOS] = {7,3};
const int degreeTime = 9;
const int maxTurnDegrees = 3;

//variaveis

SoftwareServo servoList[MAX_SERVOS];
int leituraSensor[MAX_SERVOS];
int localizacaoServo[MAX_SERVOS];
int ultima_leitura = 0;

#define DEBUG 0



void inicializa_servos(){
    for(int i = 0; i < MAX_SERVOS; i++){
          servoList[i].attach(servoInput[i]);
    }

    #if defined(DEBUG) && DEBUG == 1
    //realiza teste do servo numero
    for(int i = 0; i < MAX_SERVOS; i++){
      servo_move(servoList[i]  , 0); //reseta posicao 0
      servo_move(servoList[i]  , 180); // vai para 180
      servo_move(servoList[i]  , 0); // volta para posicao inicial
    }
    #endif

    for(int i = 0;i < MAX_SERVOS; i++){
        servoList[i].detach();
    }


}


byte servo_angulo_atual(SoftwareServo& servo){
        return servo.read();
}





void servo_move(SoftwareServo& servo,  int angulo){
        byte angulo_atual;


        angulo_atual = servo.read();

        if (angulo > angulo_atual){
              for(int ang = angulo_atual;  ang < angulo; ang++ ){
                  servo.write(ang);
                  servo.refresh();
                  delay(50);
              }
        }else {
            for(int i = angulo_atual; i > angulo; i-- ){
                servo.write(i);
                servo.refresh();
                delay(50);
            }
        }
}


long pegaDistancia(int trigger, int echo){
    int duration = 0;
    long distancia; //  em centimetros

    digitalWrite(trigger, LOW);
    delayMicroseconds(2);

    digitalWrite(trigger,  HIGH);
    delayMicroseconds(5);

    digitalWrite(trigger, LOW);
    duration = pulseIn(echo, HIGH);
    duration  = constrain(duration, sensorPerto, sensorLonge);


   distancia = (duration / 2) / 29.1;

  //ret = map(dur, sensorPerto, sensorLonge, 0, 100);

  return distancia;
}


void inicializa_sensores(){
      for(int i = 0; i < MAX_SERVOS; i++){
          pinMode(sensorInputTrigger[i], OUTPUT);
          pinMode(sensorInputEcho[i], INPUT);
      }
}

void setup(){

  inicializa_sensores();
  inicializa_servos();


}




void loop(){
      Serial.begin (9600);
      unsigned long delay_time;
      int leitura_anterior;

      for(int i =0; i < MAX_SERVOS; i++){
           leituraSensor[i] = pegaDistancia(sensorInputTrigger[i], sensorInputEcho[i]);


              //servo_move(servoList[i],  localizacaoServo[i]);

              leitura_anterior = localizacaoServo[i];
              localizacaoServo[i] =   map(leituraSensor[i],  0, 100, servoPosPerto[i], servoPosLonge[i]);


              if( abs( localizacaoServo[i] - leitura_anterior) >= maxTurnDegrees  ){
                      servoList[i].attach(servoInput[i]); // ligamos o servo
                      delay(10);

                      servo_move(servoList[i], localizacaoServo[i]);
                      delay_time = ( degreeTime * (abs( localizacaoServo[i] - leitura_anterior)) + 20 );

                      delay(delay_time);
                      servoList[i].detach(); // desligamos o servo
                      localizacaoServo[i] = leitura_anterior;
               }else {
                    servoList[i].attach(servoInput[i]);
                    delay(10);
                    servo_move(servoList[i] , leitura_anterior);
                    servoList[i].detach();
                    delay(50);
                    localizacaoServo[i] = leitura_anterior;
               }
      }

  delay(50);
}