Smart Robot Sensor/Servo

Programm zum Messen des Abstandes in verschiedene Richtungen.

#include
Servo servoblau;

int entfernung=0;
long messung=0;
int echoPin=A4;
int triggerPin=A5;
long messung160=0;
long messung40=0;
long messung100=0;
int test = 0;




void setup() {
  // put your setup code here, to run once:
Serial.begin(9600); // Serielle Kommuntikation starten
pinMode(triggerPin, OUTPUT);
pinMode(echoPin, INPUT);
servoblau.attach(3);
}


void loop() {
  // put your main code here, to run repeatedly:
digitalWrite(triggerPin, LOW);
delayMicroseconds(2);

digitalWrite (triggerPin, HIGH);
delay (10);
digitalWrite(triggerPin, LOW);

messung = pulseIn(echoPin, HIGH);

entfernung = (messung/2) / 29;

  Serial.println (entfernung);
  delay (60);
 


if (entfernung <= 50)
{

delay(1000);
      servoblau.write(160); //Position 2 ansteuern mit dem Winkel 160°

     delay(1000); //Das Programm stoppt für 3 Sekunden
     
     digitalWrite(triggerPin, LOW);
     delayMicroseconds(2);

     digitalWrite (triggerPin, HIGH);
     delay (10);
     digitalWrite(triggerPin, LOW);
     messung160 = pulseIn(echoPin, HIGH)/58;
     Serial.println(messung160);
 
     servoblau.write(40); //Position 3 ansteuern mit dem Winkel 40°
        delay(1000);//Das Programm stoppt für 3 Sekunden
        delayMicroseconds(2);

     digitalWrite (triggerPin, HIGH);
     delay (10);
     digitalWrite(triggerPin, LOW);
 
      messung40 = pulseIn(echoPin, HIGH)/58;
      Serial.println(messung40);

    servoblau.write(100); //Position 4 ansteuern mit dem Winkel 100°
       delay(1000);//Das Programm stoppt für 3 Sekunden
       delayMicroseconds(2);

     digitalWrite (triggerPin, HIGH);
     delay (10);
     digitalWrite(triggerPin, LOW);

    messung100 = pulseIn(echoPin, HIGH)/58;
    Serial.println(messung100);
delay(1000);

    if ( messung160>messung40 ){
      if ( messung100>messung160 ){
      //wenn 100 Grad am grössten ist
      test = 1;
    }
      else
      {//wenn 160 Grad am grössten ist
      test = 1;
      }

    if (messung40>messung160 and messung40>messung100)
  {  //wenn 40 Grad am grössten ist
    test = 1;

    
  }
else
{
  servoblau.write(100);
}
}
}
}
 

Von: Mathias Mohs, Dominik Hartung, Lukas Schadl, Max Wolter, Jakob Kalff