/*
 * Dieser Sketch, steuert die 4 Motoren sowie den Ultraschallsensor (Abstandsmessung),
 * welcher auf einem Servomotor montiert ist, sodass er den Abstamd in versch.
 * Richtungen messen kann.
 *
 * Motor:
 *        - Pin4: rechts Rückwärts
 *        - Pin5: rechts Vorwärts
 *        - Pin6: links Rückwärts
 *        - Pin7: links. Vorwärts
 * Ultraschallsensor:
 *        - Pin10: triggerPin
 *        - Pin9: echoPin
 * Servomotor:
 *        - Pin3: Signal Servo
 *
  */


#include                  //Einbinden der Library zur Steuerung des Servomotors

int trigPin = 10;
int echoPin = 9;                   //Defninition der Pins, die für den Ultraschallsensor benötigt werden
int rf = 5                         //right forward; Pin 5; durch ein Signal an Pin 5, 
                                   //drehen sich die beiden rechten Räder vorwärts
int rb = 4                         //right backward; Pin 4; " Pin 4 " rückwärts
int lf = 7                         //left forward; Pin 7; " Pin 7 drehen sich die beiden linken Räder vorwärts
int lb = 6                         //left backward; Pin 6; " Pin 6 " rückwärts
int svs = 3                        //Servomotor - Signal; Pin 3
int servonormal = 90               //Normalstellung des Servomotors  -  überprüfen!!!!
int servolinks = 0                 //Servo ist nach links gedreht  -  überprüfen!!!!
int servorechts = 180              //Servo ist nach rechts gedreht  -  überprüfen!!!!
int drehzeit = 100                 //Zeit in Millisec, die der Roboter für die Drehung benötigt - überprüfen!!!!

void setup() {
  // put your setup code here, to run once:
Serial.begin (9600);
pinMode(1, OUTPUT);               //Definiere die u.g. Pins als OUTPUT
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(9, INPUT);                //Definiere Pin 9 als INPUT (echoPin Ultraschall)
pinMode(10, OUTPUT);
pinMode(11, OUTPUT);
pinMode(12, OUTPUT);
pinMode(13, OUTPUT);
servo.write(servonormal);         //Am Anfang wird der Servomotor in die Neutralstellung gebracht
delay(500);
}

void loop() {
  // put your main code here, to run repeatedly:
digitalWrite(lf,1);
digitalWrite(rf,1);               //Der Roboter fährt mit allen 4 Rädern vorwärts

   //Ultraschallsensor:
   long dauer, distance
   digitalWrite(trigPin,0);
   delayMicroseconds(2);
   digitalWrite(trigPin,1);
   delayMicroseconds(10);
   digitalWrite(trigPin,0);       //Der Ultraschallsensor soll während der Fahrt kontrollieren, ob der Weg frei ist

   dauer = pulseIn(echoPin,1);
   distance = (dauer/2) / 25;     //Berechnung der Distanz
   Serial.println(distance);      //Die errechnete Distanz wird auf dem seriellen Monitor protokolliert
   


if(distance <= 5)
  {
    digitalWrite(rf,0);
    digitalWrite(lf,0);   //Der Roboter stoppt

      //Der Servomotor dreht den Ultraschallsensor und misst die jeweilige Entfernung zum nächsten Objekt
      servo.write(servolinks);                  //Der Servo dreht den Ultraschallsensor nach links
         long dauerlinks, distancelinks
         digitalWrite(trigPin,0);
         delayMicroseconds(2);
         digitalWrite(trigPin,1);
         delayMicroseconds(10);
         digitalWrite(trigPin,0);

         dauerlinks = pulseIn(echoPin,1);
         distancelinks = (dauer/2) / 25;        //Die errechnete Distanz erscheint auf dem seriellen Monitor
         Serial.println("Entfernung links:");
         Serial.println(distancelinks);
         delay(200);

      servo.write(servorechts);                  //Der Servo dreht den Ultraschallsensor nach rechts
         long dauerrechts, distancerechts
         digitalWrite(trigPin,0);
         delayMicroseconds(2);
         digitalWrite(trigPin,1);
         delayMicroseconds(10);
         digitalWrite(trigPin,0);

         dauerrechts = pulseIn(echoPin,1);
         distancerechs = (dauer/2) / 25;        //Die errechnete Distanz erscheint auf dem seriellen Monitor
         Serial.println("Entfernung rechts:");
         Serial.println(distancerechts);
         delay(200);

       servo.write(servonormal);  

      //Vergleich beider Messwerte:
      if(distancerechts > distancelinks)    //Wenn rechts mehr Platz ist als links...
        {
          digitalWrite(lf,1);
          digitalWrite(rb,1;
          delay(drehzeit);
          digitalWrite(lf,0);
          digitalWrite(rb,0);
          delayMicroseconds(100);
          digitalWrite(lf,1);
          digitalWrite(rf,1);         //Der Roboter dreht sich nach rechts und fährt weiter
        }
      else                            //Wenn die if-Bedingung nicht erfüllt ist, muss links mehr Platz sein 
                                        -> Der Roboter soll nach links fahren
        {
          digitalWrite(lb,1);
          digitalWrite(rf,1;          //Drehung
          delay(drehzeit);            //Drehzeit muss noch definiert werden
          digitalWrite(lb,0);
          digitalWrite(rf,0);         //STOP
          delayMicroseconds(100);
          digitalWrite(lf,1);
          digitalWrite(rf,1);         //Der Roboter dreht sich nach links und fährt weiter
        }
  }
else
  {
    delay (490);
    continue
    /*
     * Wenn die erste if-Bedingung nicht erfüllt ist - d.h. kein Hindernis zu erkennen ist, 
     * soll nach insg. 500 Millisekunden erneut gemessen werden
     */
  }


delay(10);
}     //Das Programm startet von vorne

/*Florian Porth_2020_10b_10WU_PÖR*/