Braccio robotico con temaglia ad attivazione ad ultrasuoni

 

Codice:

#define TRIG_PIN 10
#define ECHO_PIN 9
#define TEMPERATURA_PIN A4
#include <Servo.h>
Servo myservo;
Servo youservo;
Servo tersoservo;
Servo quartoservo;
Servo quintoservo;
int potenziometro= A0;
int potenziometroprimo= A1;
int potenziometroterso= A2;
int potenziometroquarto= A3;
int bottone = 7;
int led = 13;
int bb;
int aa;
int cc;
int dd;
int ee;
int ff;
int val;
int pos;
int gio;
int anni;

void setup(){

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  digitalWrite(TRIG_PIN, LOW);
  Serial.begin(9600);
  myservo.attach(2);
pinMode(potenziometro,INPUT);
  youservo.attach(3);
  pinMode(potenziometroprimo,INPUT);
  tersoservo.attach(4);
  pinMode(potenziometroterso,INPUT);
  quartoservo.attach(5);
  pinMode(potenziometroquarto,INPUT);
  quintoservo.attach(6);
   quintoservo.write(0);
  pinMode(bottone, INPUT);
  pinMode(led, OUTPUT);

}

void loop(){
  int input = analogRead(TEMPERATURA_PIN);
  float temperatura = input / 2.046;

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long tempo = pulseIn(ECHO_PIN, HIGH);
  float velocita = 0.03314 + 0.000062 * temperatura;
  float distanza = velocita * tempo / 2;

  Serial.println("Distanza: " + String(distanza) + "cm");

  aa=analogRead(potenziometroterso);
  bb=map(aa, 0, 1023, 0, 180);
  tersoservo.write(bb);
 
  cc=analogRead(potenziometroquarto);
  dd=map(cc, 0, 1023, 0, 180);
  quartoservo.write(dd);
  /////////////////////////////

  if(distanza <9){
    digitalWrite(led,HIGH);
      delay(300);
      digitalWrite(led,LOW);
      delay(300);
      digitalWrite(led,HIGH);
      delay(300);
      digitalWrite(led,LOW);
      delay(300);
      digitalWrite(led,HIGH);
      delay(300);
      digitalWrite(led,LOW);
     
   quintoservo.write(180);
 
  }
 
  ////////////////////////////
  if(digitalRead(bottone) == HIGH){
   
   quintoservo.write(0);
  }
 
  val=analogRead(potenziometro);
  pos=map(val, 0, 1023, 0, 180);
  myservo.write(pos);
 
 gio=analogRead(potenziometroprimo);
   anni=map(gio, 0, 1023, 0, 180);
  youservo.write(anni);
}