Farb-Sortiermaschine mit einem Microcontroller und Farbsensor

Hier mein erster Versuch das kunterbunte treiben, meiner farbigen Schokolinsen in den Griff zu bekommen.
Eine Sortiermaschine sollte es werden, nach einer Idee von howtomechatronics

An einem Elegoo Board wurde ein Farbsensor TSC3200 und zwei Servo Motoren angeschlossen.
An den ersten Servo´s habe ich eine Pappscheibe angeklebt, die das abholen einzelner Linsen gewährleistet und diese möglichst flach dem Sensor vor die Nase legt.
Der Sensor prüft die Farbe und bestimmt somit die Drehrichtung des zweiten Servo´s.
Der zweite Servo dreht die Rutsche in die Richtung des richtigen Ablagefach.
Damit ich die Servo´s an der Fischertechnikkonstruktion befestigen konnte wurde mit dem 3D Drucker eine Adapterummantelung gedruckt.
Druckvorlage für Servo auf Fischertechnik: https://www.thingiverse.com/thing:2807112
So war meistens eine Farbsortierung möglich.

Um so dunklere der Raum, in dem dieser Aufstand steht, um so Treffsicherer war die Farbsortierung.
Somit sollte der Teil mit dem Farbsensor besser abgedunkelt werden, als hier zu sehen.

Hardware:
Board Elegoo 2560 R3
Farbsensor TSC3200
2x SG90 Mini Servo
diverse Fischertechnik Bauteile
3D Drucker ANet A8 für Adapter

…und immer wieder das Nachkaufen neuer Schokolinsen, die auf geheime Weise immer wieder weniger wurden (Seltsam)

//M und M Sortierer V2

#include <Servo.h>

#define S0 4
#define S1 5
#define S2 6
#define S3 7
#define sensorOut 8

Servo topServo;
Servo bottomServo;

int frequency = 0;
int color=0;

void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);

  // Setting frequency-scaling to 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);

  topServo.attach(9);
  bottomServo.attach(10);

  Serial.begin(9600);
}

void loop() {

  topServo.write(115); //übernahme des opjektes
  delay(500);
  
  for(int i = 115; i > 65; i--) {
    topServo.write(i);
    delay(100);
  } // weg zu messung
  delay(500);
  
  color = readColor();
  delay(100);  
Serial.println(color);//printing name
  switch (color) {
    case 1:
    bottomServo.write(50);
    break;

    case 2:
    bottomServo.write(75);
    break;

    case 3:
    bottomServo.write(100);
    break;

    case 4:
    bottomServo.write(125);
    break;

    case 5:
    bottomServo.write(150);
    break;

    case 6:
    bottomServo.write(175);
    break;
    
    case 0:
    break;
  }
  delay(300);
  
  for(int i = 65; i > 29; i--) {
    topServo.write(i);
    delay(2);
  } 
  delay(200);
  
  for(int i = 29; i < 115; i++) {
    topServo.write(i);
    delay(2);
  }
  color=0;
}

// Custom Function - readColor()
int readColor() {
  // Setting red filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int R = frequency;
  // Printing the value on the serial monitor
  Serial.print("R= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  delay(50);

  // Setting Green filtered photodiodes to be read
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int G = frequency;
  // Printing the value on the serial monitor
  Serial.print("G= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  delay(50);

  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int B = frequency;
  // Printing the value on the serial monitor
  Serial.print("B= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.println("  ");
  delay(50);

  if(R<B && R<G) {
    color = 1; // Red
  }
  if(R<137 & R>80 & G<142 & G>119 & B<127 &B>117){
    color = 2; // Orange
  }
  if(G<R && G<B){
    color = 3; // Green
  }
  if(R<100 & R>70 & G<121 & G>100){
    color = 4; // Yellow
  }
  if(R<138 & R>122 & G<172 & G>144 & B<147 & B>125){
    color = 5; // Brown
  }
  if(B<R && B<G){
    color = 6; // Blue
  }
  return color;  
}
, , ,