#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;
}