segunda-feira, 2 de maio de 2016

Controle de abrir e fechar a garra

#include <Servo.h>

Servo garra_d;
Servo garra_e;
Servo elevador;

int emissor=10;
int receptor=9;
long cm, duracao;

int aberto_d= 0;
int aberto_e= 0;
int fechado_d=0;
int fechado_e=0;
int abaixa = 0;
int levanta = 90;

int x;
int distancia;

void setup(){
pinMode (emissor, OUTPUT);
pinMode (receptor, INPUT);

garra_d.attach (9);
garra_e.attach (10);
elevador.attach (11);

garra_d.write (aberto_d);
garra_e.write (aberto_e);
elevador.write (levanta);
}

void loop(){

distancia = sonar();
  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  delay(100);

if (distancia>5 cm){
Serial.println ("sem objeto a frente");
garra_d.write (aberto_d);
garra_e.write (aberto_e);
elevador.write (levanta);
digitalWrite (13, HIGH);
}

if (distancia<5 cm){
Serial.println ("objeto a frente");
digitalWrite (13, LOW);
elevador.write (abaixa);
garra_d.write (fechado_d);
garra_e.write (fechado_e);
}

}

int sonar (){
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(pingPin, LOW);

  duracao = pulseIn(pingPin, HIGH);

  cm = microsecondsToCentimeters(duracao);


}


long microsecondsToCentimeters(long microseconds) {
  return microseconds / 29 / 2;
}

Nenhum comentário:

Postar um comentário