// 
// Steuerprogramm für einen kleinen Roboterarm.
//
// Autor: Makerspace Gießen
//
// Lizenz: Creative Commons Public Domain - oder auch: Mach damit was du willst ;)
//
// Alle Infos zum Makerspace in Gießen und www.makerspace-giessen.de
//




// Über "include" wird die Servo Bibliothek eingebunden. 
// In ihr sind die notwendigen Funktionen enthalten, um die kleinen Servo-Motoren anzusteuern.

#include <Servo.h> 

// Mit "Servo" erstellt man ein Servo-Objekt mit einem beliebigen Namen. 
// Diese Objekte werden später zum Steuern der Servos genutzt.
// Wir wählen Namen passend zur Position später noch wissen, 
// welcher Servo an welcher Stelle sitzt.

Servo servo_unten; //Servo drehen
Servo servo_1; // 1. Arm
Servo servo_2; // 2. Arm
Servo servo_greifer; // Greifer

// Wir legen noch ein paar Variablen an, die wir später brauchen. 
// Das macht man meistens vor dem restlichen Code.

int Stellwert_unten = 90;
int Stellwert_1 = 90;
int Stellwert_2 = 90;
int Stellwert_Greifer = 90;




// Jetzt kommt unser setup, in dem wir die Servos den einzelnen Pins zuweisen und einen kleinen
// Funktionstest durchführen.

void setup() {

  // Hier werden die Servos ihren Anschluss-Pins zugeordnet
  servo_unten.attach(5);
  servo_1.attach(9);
  servo_2.attach(10);
  servo_greifer.attach(11);


  // Zum Spaß bauen wir auch wieder eine serielle Kommunikation mit dem PC auf.
  
  Serial.begin(9600); // Serial comm begin at 9600bps
  Serial.println("Los gehts!");

  // Roboterarm in Ausgangsstellung fahren.
  // Die Funktion servoName.write(Zahl) funktioniert über Winkel zwischen 0 und 180 Grad.

  servo_unten.write(90);
  servo_1.write(90);
  servo_2.write(90);
  servo_greifer.write(0);

   // Arm drehen
  servo_unten.write(10); 
 
 // Arm runter fahren
    Serial.println("Arm wird runtergefahren!");

 
  delay(500);
  servo_1.write(10);
  delay(500);
  servo_2.write(180);
  delay(1500);

  // Arm hoch fahren

  Serial.println("Arm wird hochgefahren!");

  servo_2.write(80);
  delay(500);
  servo_1.write(50);
  delay(500);

  
  // Arm drehen
  servo_unten.write(170);

 Serial.println("Setup abgeschlossen! Hauptprogramm wird gestartet!");

}





// Ab hier geht es in das Hauptprogramm - die Dauerschleife loop()

void loop() {

  // Sensor-Wert von Pin A0 einlesen und in der Variable SensorWert speichern
  int SensorWert = analogRead(A0);

  // Über die serielle Schnittstelle geben wir den Sensorwert rein zu Lernzwecken an den PC, so dass wir später am Monitor sehen können welcher Wert eingestellt ist.
  
  Serial.println(" ");
  Serial.println("Ich habe folgenden Sensorwert gemessen:");
  Serial.println(SensorWert);

  // Jetzt wird der Roboterarm bei niedrigem Sensorwert heruntergefahren und bei hohem Sensorwert soll er sich aufrichten. 
  // Dafür verknüpfen wir die Sensorwerte mit den Stellwerten für die Servos. Dahinter steckt kein höheres Prinzip, 
  // hier haben wir einfach viel rumprobiert, bis die Werte gepasst haben ;)
  // Wir nutzen dafür die Funktion map(). Wenn du dazu mehr wissen willst, schau in die Arduino Reference.

  Stellwert_unten = map(SensorWert, 110, 800, 130, 50);
  Stellwert_1 = map(SensorWert, 110, 800, 130, 50);
  Stellwert_2 = map(SensorWert, 110, 800, 130, 50);
  Stellwert_Greifer = map(SensorWert, 110, 800, 130, 50);

  // Kleine Rückmeldung an uns auf dem PC-Bildschirm ausgeben:
  
  Serial.println(" ");
  Serial.println("Ich habe aus dem Sensorwert folgende Stellwerte der Servos errechnet:");
  Serial.println(SensorWert);


  // Die Servos in die berechneten Position fahren. Dabei jeweils kurz warten, 
  // damit es 1. schöner aussieht und 2. wir keine Überlastung erzeugen. 
  // Der Arduino kann nicht so viel Strom gleichzeitig liefern.
  
  servo_unten.write(Stellwert_unten);
  delay(50);
  servo_1.write(Stellwert_1);
  delay(50);
  servo_2.write(Stellwert_2);
  delay(50);
  servo_greifer.write(Stellwert_Greifer);
  
  Serial.println(" ");
  Serial.println("Roboterarm in neue Position gefahren. Und wieder von vorne!");
  Serial.println(" ");

  // Kurz warten, bevor wir wieder oben am Anfang der loop()-Funktion anfangen.
  delay(10);
  



}