Projektlabor Robotik MINTgrün

	  #include <Servo.h> // Import der Servo-Libraries 
	
	  //TODO: Startknopf einbauen Eingan A3
	
	  /** MERKZETTEL:
	
	  malloc() nachschlagen. Koennte uns sehr mit RAM helfen.
	  Beispiel: ptr = malloc(100)  ...oder so.
	
	  **/

	  // Pins

		// Start-Knopf

		const int startButton = A3;
	
		//  Stepper - Motoren
		/* 
		   Zusatz-Infos zu den Steppern:
   
			Pololu 1 für den Linken Stepper befindet sich direkt neben dem Arduino, 
			Pololu 2 für den Rechten Stepper ist der Andere.
			Den Linken Stepper so anbringen, dass das schwarze Kabel direkt neben dem 
			Kondensator ist, den Rechten so, dass das blaue Kabel direkt daneben ist.
		*/

		const int dirL = 2;   //Linker Stepper
		const int stpL = 3;   
		const int dirR = 7;  //Rechter-Stepper
		const int stpR = 9;   
		const int stepDelay = 15; // Delay zwischen steps. Steuer die Geschwindigkeit
		const double stepLength = 1.414; // Länge eines Schrittes mit den 90mm Rädern.
		boolean driveStraight = false; 
 	
		// Sonar 

		const int inPinSonar = A4;
		const int pingPinSonar = 12;

		// Arm-Servos für Drehbewegung + Heben

		const int rotor = 6;     //mittlerer Anschluss
		const int lifter = 4;    //innerer Anschluss

		// Servo mit Ventil-Funktion - 0 ist weitestmöglich Richtung geschlossen, 180 ist weitestmöglich geöffnet.

		const int vent = 5;  //äußerer Anschluss
		const int valveShut = 40; // Grandzahl, bei welcher der Servo vollständi geschlossen ist.
		const int valveOpen = 180;

		//  LEDs 

		const int led = 12; // LED für die Beleuchtung der Steuerungs-Phototransistoren
		const int resetLed = 13; // LED auf dem Arduino, soll Leuchten, wenn der Arduino abstürzt und sich resetted

		//  Phototransistoren 

		const int photoL = A6;
		const int photoR = A7; // Das blaue Kabel

		// Kontakt für Fuellstand, Erdung G56 (schwarz), Einspeisung G55 (rot)

		const int fuellstandsPin =A5;


		//  Konstanten

		const int armLength = 235; //  Armlänge in mm
		const int armOffAxis = 30; //TODO: einbauen
		const double pi= 3.15;
		const int threshold = 1020; //  für Fuellstanderkennung TODO: testen
		const int distSonar = 310; 
		const int radiusGlas = 20;
	
	
		//  Variablen  

		//  für Glaserkennung
		long length = 0; // Speichert die Anzahl der Schritte, die der Roboter an einem Glas vorbei fährt
		boolean found = false;    //Boolean, ob Roboter an einem Glas vorbei fährt
		boolean passed = false;    //Boolean, ob Roboter ein Glas passiert hat 
		int glassDistanz = 0;
		int glassCounts = 0;
 	
		//  für Fahrfunktion
		boolean rightWasLast = false;     //Boolean, ob Roboter vorher nach rechts gefahren ist
		boolean leftWasLast = false;      //Boolean, ob Roboter vorher nach links gefahren ist
		int brightnessLeft = 0;        //Wert des Phototransitoren auf der linken Seite
		int brightnessRight = 0;      //Wert des Phototransistoren auf der rechten Seite
		const int lightThreshold = 150;     // Abfrage, wie viel heller der helle Bereich ist, als die schwarze Linie
		const int lightBlack = 600;       // Lichtintensität, ab welcher wir von Erkennung der schwarzen Linie ausgehen
		const int photoCorrection = 100; // Korrektur, die auf den rechten Phototransistor angewendet wird.

		//  Servos
		Servo rotate;
		Servo lift;
		Servo ventil;

		void setup(){
  
		  //TODO Serielle Ausgabe entfernen
		  Serial.begin(9600);
		  Serial.println("ABSTURZ oder vielleicht auch erster Start");      // Arduino-LED: Leuchtet auf, wenn der Arduino neu startet.      
  
		  pinMode(resetLed,OUTPUT);
		  digitalWrite(resetLed,HIGH);
		  delay(100);
		  digitalWrite(resetLed,LOW);    
  
  
		  //Initialisierungen

		  // Steuerungs-LED anmachen
		  pinMode(led,OUTPUT);  
		  digitalWrite(led,HIGH);

		  // Arm-Servos anschließen und ausrichten
		  rotate.attach(rotor, 800, 2100);
		  lift.attach(lifter, 800, 2100);

		  resetArm();    

		  // Ventil-Servo anschließen TODO: ausrichten - benötigt wahrscheinlich nicht die Korrekturwerte
		  ventil.attach(vent);
		  ventil.write(valveShut); // Ventil schliessen. TODO Wert gegen valveShut ersetzen.

		  // Stepper-Motoren initialisieren
		  pinMode(dirL, OUTPUT);
		  pinMode(stpL, OUTPUT);
		  pinMode(dirR, OUTPUT);
		  pinMode(stpR, OUTPUT);
 	
		  // Stepper-Motoren Richtung auf vorwärts stellen. 
		  digitalWrite(dirL,LOW);
		  digitalWrite(dirR,LOW);
  
		  // TODO testen, ob der Startknopf so funktioniert.
		  while(analogRead(startButton)<1020) delay(1);

		}
	
	  void loop(){
			Glaserkennung(); // prüfe, ob sich ein Glas vor dem Sonar befindet.
			/*
			Merke: Jedes Objekt, das auf weniger als die Armlänge des Roboters
			von ihm entfernt zu seiner Rechten aufgestellt ist, wird als Glas erkannt.
			*/
	
	
			if(found){ // found ist true, wenn ein Glas entdeckt wurde.
			  Serial.println("Glas gefunden");
		  driveStraight = true;
			  if(passed){ // wird ausgeführt, wenn ein Glas gefunden und vom Sonar passiert wurde
				 Serial.println("Glas passed");
				 glassDistanz = glassDistanz/glassCounts;
				 Serial.print("GlassDistanz = ");
				 Serial.println(glassDistanz);
				 double h = calcH();   //fahre restliche Distanz 
				 Serial.print("h = ");
				 Serial.println(h);
				 double steps = convertToSteps(h)-(length/2);
				 Serial.print("steps = ");
				 Serial.println(steps);
				 Glaserkennung();    //damit drive length nicht abändert
				 for(int i=0; i<steps; i++){
				   drive();
				 }
				 moveArm(glassDistanz+radiusGlas);   //Bewege Arm
				 fill();   //Füllen
				 resetArm();   //resetten     
				 length=0;           //length
				 glassDistanz = 0;  
				 glassCounts = 0;
		
			  }else{ // wird ausgeführt, wenn ein Glas gefunden aber noch nicht vollständig vom Glas passiert wurde
				for(int i = 0; i<5; i++)  drive();   //fahre und zähle Schritte
			  }

			}else{ // wird ausgeführt, wenn noch kein Glas gefunden wurde
			  driveStraight = false;
			  for(int i = 0; i<5; i++) drive(); //fahre bis zum nächsten Glas
			}
	


	  }

	  /*
		Diese Methode setzt uns passed und found.
		D.h. wir überprüfen, ob wir an einem Glas vorbei fahren, es passieren oder kein Glas in der Nähe ist.
		Außerdem wird hier die Entfernung zum Glas mitgeschrieben - glassDistanz wird später durch Mess-Anzahl dividiert.
	   */
	  void Glaserkennung(){
		long distance = distanceSonar();
		Serial.print("distance = ");
		Serial.println(distance);
		if(distance > 0) {
		  found = true;
		  glassDistanz = glassDistanz + distance;
		  glassCounts++;
		}                                //wir haben ein Glas gefunden
		else if(found &&!passed && distance == 0) passed = true;    //gerade am Glas vorbei                                     
		else if(found && passed && distance ==0){                  //wir haben das Glas bereits passiert=> auf Anfang setzen
		  passed = false; 
		  found = false;
		}
	  }
	
	
	  /*
		Methode, welche mit dem Sonar die Distanz zu etwaigen Objekten überprüft und uns in mm zurück gibt.
		Gibt es kein Objekt in direkter Nähe, wird 0 zurück gegeben, 
	   */
	  long distanceSonar(){
	
		// Initialisiere Variablen für die Distanz 
		// duration sind ms und mm sind millimeter
		long duration = 0;
		long mm;
		for(int i=0; i<10;i++){
		  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
		  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
		  pinMode(pingPinSonar, OUTPUT);
		  digitalWrite(pingPinSonar, LOW);
		  delayMicroseconds(2);
		  digitalWrite(pingPinSonar, HIGH);
		  delayMicroseconds(10);
		  digitalWrite(pingPinSonar, LOW);
	
		  // The same pin is used to read the signal from the PING))): a HIGH
		  // pulse whose duration is the time (in microseconds) from the sending
		  // of the ping to the reception of its echo off of an object.
		  pinMode(inPinSonar, INPUT);
		  duration = duration + pulseIn(inPinSonar, HIGH);
		  delay(25);
		 }
		// in mm umrechnen 
		mm = microsecondsToMillimeters(duration/10); 

		//TODO Schwelle erhöhen, falls möglich.
		if(mm > 160) mm = 0;
	
		return mm;
	  }
	
	  /*
		Methode, die den Arm in Ausgangsposition bringt.
	  */
	  void resetArm(){ 
		lift.write(100);  //TODO: nach oben zeigen lassen
		rotate.write(175);
	  }

	  /*
		Methode, welche die Armbewegung ausführt, d.h. ausfahren und senken.
	  */
	  void moveArm(double distance){
	  // TODO die Servos machen keine vollen 180° - das müssen wir manuell messen
	  // TODO hier müssen wir noch die Distanz des Sonar-Sensors auf die Achse des Rotators normieren

		double alpha = asin(distance/armLength); // Berechnung der Arm-Position bei gegebenem Abstand
  
		alpha = (alpha/pi)*180; // Umrechnung von Rad in Deg    

		delay(500L);
		rotate.write( (int) 175-alpha);    //Drehung
		delay(500L);
		lift.write(40); //TODO: Testen ob das gut ist //Senkung
	  }
	
	  /*
		Restlänge, die Der Roboter zurücklegen muss, nachdem das Glas vom Sonar passiert wurde, aber ehe der Arm ausgefahren wird.
		TODO: Wird hier beachtet, wie viele Schritte wir bereits mit dem Sonar am Glas vorbeigefahren sind? -Rene
	  */
	  double calcH(){
		double g = glassDistanz + radiusGlas; //Distanz von Roboter zu Glasmitte
		Serial.print("g = ");
		Serial.println(g);
		double square = round(sqrt(sq((double) armLength) - sq(g))); // Wir casten hier auf Double, weil Arduino behindert ist.
		Serial.print("sqrt(armLength*armLength - g*g) = ");
		Serial.println(square);
		return distSonar - square; 

	  }
	
	  /*
		Methode, die anhand der Elektroden am Zapfarm erkennt, ob das darunter befindliche Glas voll ist.
		Gibt bei Kontakt der Elektroden mit der Flüssigkeit (also Glas voll) true aus, sonst false.
	  */
	  boolean fuellstand(){
		 //Wenn Strom fließt, ist die Flüssigkeit hoch genug, um Strom zu leiten
		if(analogRead(fuellstandsPin) <= threshold) return false; 
		else return true;

	  }
	

	  /*
		Öffnet das Ventil, wartet bis das darunter befindliche Glas voll ist, schließt das Ventil wieder.
	  */
	  void fill(){
		ventil.write(valveOpen);
		while(!fuellstand())  delay(100);
		ventil.write(valveShut);
	  }

	
	  /*
		Methode Um MicroSekunden in mm umzurechnen. 
		Schallgeschwindigkeit liegt bei 340 m/s oder 29 mikrosekunden/cm. 
		Der Ping geht den Weg zum Objekt und zurück, also rechnen wir durch 2.
	  */
	  long microsecondsToMillimeters(long microseconds){
		 return (microseconds / 2.9 / 2);
	  }


	  /*
		Methode um an der Linie entlang zu fahren, Achtung: ändert die Variable length.
		rightWasLast und leftWasLast bleiben auch zwischen Iterationen der Methode erhalten,
		daher kann der Roboter auch nachdem die Linie überhaupt nicht mehr gesehen wird zu ihr
		zurückfinden.
		Der boolean driveStraight wird im Loop auf true gesetzt, wenn wir zu Messzwecken oder um ein
		Glas zu erreichen nur genau geradeaus fahren dürfen. Danach findet der Roboter trotzdem zur 
		Linie zurück.
		Mit dem int stepDelay lässt sich die Geschwindigkeit des Roboters regulieren.
	  */
   	 
      	  void drive(){
   	
		 brightnessLeft = (analogRead(photoL) + analogRead(photoL))/2;
		 brightnessRight = (analogRead(photoR) + analogRead(photoR))/2 + photoCorrection;

	
		 //Überprüfe mit den Phototransitoren, wo die Linie ist 
		 //z.B. wenn Links mehr weiß ist als rechts, dann müssen wir von rechts über die Linie gefahren sein
		 if(brightnessLeft> brightnessRight + lightThreshold){  
			rightWasLast = true;
			leftWasLast = false;
		  } 
		  else if (brightnessRight > brightnessLeft + lightThreshold ){
			leftWasLast = true;
			rightWasLast = false;
		  } 
		  else if(brightnessRight < lightBlack && brightnessLeft < lightBlack){
			leftWasLast = false;
			rightWasLast = false;
		  }
  
		  //Fahrbewegung
		  if(rightWasLast && !driveStraight){   //nach rechts fahren
			digitalWrite(stpL, HIGH); 
			delay(1);               
			digitalWrite(stpL, LOW);  
			delay(stepDelay);
		  }
		  else if(leftWasLast && !driveStraight){   //nach links fahren
			digitalWrite(stpR, HIGH);   
			delay(1);               
			digitalWrite(stpR, LOW);  
			delay(stepDelay);
		  }
		  else if((!rightWasLast && !leftWasLast) || driveStraight){   // geradeaus fahren
			digitalWrite(stpL, HIGH);
			digitalWrite(stpR, HIGH);   
			delay(1);
			digitalWrite(stpL, LOW);                 
			digitalWrite(stpR, LOW);  
			delay(stepDelay);
		  } 


		  //Wenn wir gerade an einem Glas vorbei fahren, zählen wir die Schritte
		 if(found && !passed){ //Merke: muss danach auf 0 gesetzt werden
			length++;
		  }
		}

		/*
		  Rechnet Stepperschritte in mm um
		*/
		long convertToMM(long steps){
		  return steps*stepLength;    
		}

		/*
		  Rechnet mm in Stepperschritte um
		*/
		long convertToSteps(long mm){
		  return mm/stepLength;
		}