Farbsortierer von fischertechnik mit ftduino gesteuert

Ein weiterer Farbsortierer, diesmal mit dem Elektro Pneumatik Bausatz von Fischertechnik erstellt, aber ohne TXT Controller, sondern in C++ in der ArduinoIDE für den Microcontroller ftduino programmiert. Wieder wurde ein Farbsensor zur Auswertung verwendet, um die Drehbewegungsstrecke vorzugeben.
Die Erkennung war diesmal zuverlässiger, da das Umgebungslicht durch das Werkstück besser abgedeckt wurde.
Nach aufspielen des Quellcode habe ich die USB-Kabelverbindung von dem PC getrennt und mit einem externen 9Volt Netzteil lief der Aufbau eigenständig.
Die Drehbewegung des großen Kranzes, wird abgetastet, durch diesen Taster:

Dadurch waren die Hin und Rückbewegungen sporadisch immer mal wieder um mehrere Millimeter versetzt. Hier habe die Problematik des Prellen (ein mechanisch ausgelöster Störeffekt bei elektromechanischen Schaltern und Tastern) kennen lernen dürfen. Anstatt dem Kontroller saubere 4 Werte pro Umdrehung zu liefern, waren es auch schon mal deutlich mehr. Besserung wurde es ein wenig, durch höhere Wartezeiten (delay).
Aber abschließend war die beste Lösung, die Umstellung auf Encoder Motoren.

Quellcode:

//Farbsortierer von Fischertechnik mit ftduino gesteuert

#include <Ftduino.h>

int los=0;
int Scheibevorhanden=0;
int istinstartposition=0;
int Farbe = 0;
int x=0;

void setup() {

ftduino.init();
ftduino.input_set_mode(Ftduino::I2, Ftduino::RESISTANCE); //Fotowiderstand
ftduino.output_set(Ftduino::O6, Ftduino::HI, Ftduino::MAX);//LED fuer Fotowiderstand einschalten
ftduino.input_set_mode(Ftduino::I3, Ftduino::VOLTAGE); // Farbsensor
ftduino.counter_set_mode (Ftduino::C1, Ftduino::C_EDGE_ANY);// Zahnrad jedes druecken und loslassen wird gezaehlt 
delay(500);
Serial.begin(115200);
//while(!Serial);
while(los < 1) {los = ftduino.input_get(Ftduino::I4) ;}  //Wartet auf Starttastendruck
Serial.print("Start");
Scheibevorhanden = ftduino.input_get(Ftduino::I2) ;
Serial.print("Scheibevorhanden");Serial.println(Scheibevorhanden);
ftduino.motor_set(Ftduino::M1, Ftduino::RIGHT, Ftduino::MAX);
do { istinstartposition = ftduino.input_get(Ftduino::I1);
if (istinstartposition > 0){break;}
} 
while ( Scheibevorhanden=!0);  //laeuft unendlich bis wert nicht null               
ftduino.motor_set(Ftduino::M1, Ftduino::OFF, Ftduino::MAX);
ftduino.counter_clear(Ftduino::C1);
Serial.print("istinstartposition");Serial.println(istinstartposition); //1=ja, 0=nein
}

void loop() {
delay(500);
 
do { Scheibevorhanden = ftduino.input_get(Ftduino::I2) ;  if (Scheibevorhanden < 100)
{break;}
} // Dadurch wird der Arm in Bewegung gesetzt, wenn eine Scheibe vorhanden ist.

while ( Scheibevorhanden > 110);  //laeuft unendlich bis wert unter 110                            
Serial.print("Scheibevorhanden: ");
Serial.println(Scheibevorhanden);
Serial.print("positionbeimgreifen ");
positionx();

armrunter(); delay(800); //Greifarm geht nach unten
saugen(); delay(800); // saugt Scheibe an
armhoch(); delay(1000); // Greifarm geht nach oben
Farbsensor(); // Greifarm geht nach Farbsensor
Serial.print("positionbeimfarbsensor ");
positionx ();
delay(500);
armrunter(); 
delay(1000);

farbestimmung(); // Farbe bestimmen
delay(500);
armhoch(); 
delay(500);

farbfach(); // zum ausgewaehlten Fach gehen
Serial.print("positionbeimfach ");
positionx ();
delay(1000);
armrunter(); 
delay(500);
loslassen(); 
delay(500);
armhoch(); 
delay(500);

startposition(); // zurueck zum start
} //ende loop

// Beginn Funktionen
void loslassen() {
ftduino.output_set(Ftduino::O3, Ftduino::OFF, Ftduino::MAX);//Kompressor aus
ftduino.output_set(Ftduino::O4, Ftduino::OFF, Ftduino::MAX);//elektromagnetische Ventil aus
}

void armrunter() {
ftduino.output_set(Ftduino::O3, Ftduino::HI, Ftduino::MAX);//Kompressor an
ftduino.output_set(Ftduino::O5, Ftduino::HI, Ftduino::MAX);//Ventilarm nach unten
}

void armhoch() {
ftduino.output_set(Ftduino::O3, Ftduino::OFF, Ftduino::MAX);//Kompressor aus
ftduino.output_set(Ftduino::O5, Ftduino::OFF, Ftduino::MAX);//Ventilarm nach oben
}

void saugen() {
ftduino.output_set(Ftduino::O3, Ftduino::HI, Ftduino::MAX);//Kompressor an
ftduino.output_set(Ftduino::O4, Ftduino::HI, Ftduino::MAX);//Ventilarm saugt
}

// weg zum Farbsensor 
void Farbsensor() {
ftduino.motor_set(Ftduino::M1, Ftduino::LEFT, Ftduino::MAX); //Motor dreht sich nach rechts
while(ftduino.counter_get(Ftduino::C1) < 5) { delay(100);}
ftduino.motor_set(Ftduino::M1, Ftduino::OFF, Ftduino::MAX);
}

// zurueck zur startposition
void startposition() {
ftduino.motor_set(Ftduino::M1, Ftduino::RIGHT, Ftduino::MAX);
do { istinstartposition = ftduino.input_get(Ftduino::I1);
if (istinstartposition > 0){break;}
} 
while ( Scheibevorhanden=!0);  //laeuft unendlich bis wert nicht mehr Null               
ftduino.motor_set(Ftduino::M1, Ftduino::OFF, Ftduino::MAX);
ftduino.counter_clear(Ftduino::C1); // zaehler zurueck setzen
Serial.print("Fertig!");
}

void farbestimmung() {
Farbe = ftduino.input_get(Ftduino::I3) ;
if ((Farbe > 1700) && (Farbe < 2100)) {x=x+11;} // blau
if ((Farbe > 1250) && (Farbe < 1699)) {x=x+9;} // rot
if ((Farbe > 600) && (Farbe < 1249)) {x=x+13;} //weiss

Serial.print("Farbwert: ");
Serial.println(Farbe); // Werte ueberpruefen, ggf anpassen
Serial.print("X:"); 
Serial.println(x); 
}

// weg zum farbfach
void farbfach(){
ftduino.motor_set(Ftduino::M1, Ftduino::LEFT, Ftduino::MAX);
while(ftduino.counter_get(Ftduino::C1) < x){delay(100); }
ftduino.motor_set(Ftduino::M1, Ftduino::OFF, Ftduino::MAX);  Serial.print("Farbfachnummer:"); Serial.println(x); 
x=0;
}

void positionx(){
Serial.print("Position: ");
Serial.println(ftduino.counter_get(Ftduino::C1));
}