Color Sorting Machine (Proiect SM)

#include <Process.h>
#include <string.h>


#define S2 27
#define S3 17
#define sensorOut 22

int frequency=0;
int culoare1[]={0,0,0,0,0,0};
int culoare2[]={0,0,0,0,0,0};
int culoare3[]={0,0,0,0,0,0};
int culoare4[]={0,0,0,0,0,0};
int cFinal=0;

String pinUp="5 ";
String pinDown="6 ";
String lastAngleUp="1.85";
String newAngleUp="0 ";
String newAngleDown="0 ";
String cale="python servoControl.py ";
String command="";

Process p;

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

  initialdetect();
  
}

void loop() {

  command="";
  newAngleUp="0.65 ";
  command =cale;
  command +=pinUp;
  command +=newAngleUp;
  command +=lastAngleUp;
    
  p.runShellCommand(command);
  while(p.running());

  lastAngleUp=newAngleUp;


  cFinal = readColor();
  delay(10);  

  switch (cFinal) {
    case 1:
      command="";
      newAngleDown="0.2 ";
      command =cale;
      command +=pinDown;
      command +=newAngleDown;
      command +=newAngleDown;
        
      p.runShellCommand(command);
      while(p.running());
    
      break;

    case 2:
      command="";
      newAngleDown="0.8 ";
      command =cale;
      command +=pinDown;
      command +=newAngleDown;
      command +=newAngleDown;
        
      p.runShellCommand(command);
      while(p.running());
      break;

    case 3:
      command="";
      newAngleDown="1.5 ";
      command =cale;
      command +=pinDown;
      command +=newAngleDown;
      command +=newAngleDown;
        
      p.runShellCommand(command);
      while(p.running());
      break;

    case 4:
      command="";
      newAngleDown="2 ";
      command =cale;
      command +=pinDown;
      command +=newAngleDown;
      command +=newAngleDown;
        
      p.runShellCommand(command);
      while(p.running());
      break;
      
    case 0:
    break;
  }
  delay(300);

  //daca readColor a returnat o culoare se va misca servo-ul
  if(cFinal!=0){
      //Mutare bomboana spre fanta coborare
      
      command="";
      newAngleUp="0.1 ";
      command =cale;
      command +=pinUp;
      command +=newAngleUp;
      command +=newAngleUp;
        
      p.runShellCommand(command);
      while(p.running());
    
      lastAngleUp=newAngleUp;

      //Revenire servo in pozitie initiala
      command="";
      newAngleUp="1.85 ";
      command =cale;
      command +=pinUp;
      command +=newAngleUp;
      command +=lastAngleUp;
        
      p.runShellCommand(command);
      while(p.running());
    
      lastAngleUp=newAngleUp;
    }
  delay(300);
  cFinal=0;

}

//Functie pentru detectarea initiala a valorilor RGB a bomboanelor
//Primele 4 bomboane introduse trebuie sa fie de culori diferite pentru functionarea corespunzatoare
void initialdetect(){
  for(int i=1;i<=4;i++)
  {
        command="";
        newAngleUp="0.65 ";
        command =cale;
        command +=pinUp;
        command +=newAngleUp;
        command +=lastAngleUp;
          
        p.runShellCommand(command);
        while(p.running());
      
        lastAngleUp=newAngleUp;

        
        int lowVal[]={0,0,0};
        int highVal[]={0,0,0};
        for(int j=1;j<=5;j++){
            int r=readRed();
            int g=readGreen();
            int b=readBlue();
            
            if(j==1){
                lowVal[0]=r;
                lowVal[1]=g;
                lowVal[2]=b;
                
                highVal[0]=r;
                highVal[1]=g;
                highVal[2]=b;
            }
            
            else{
                if(r<lowVal[0])
                    lowVal[0]=r;
                if(r>highVal[0])
                    highVal[0]=r;
                
                if(g<lowVal[1])
                    lowVal[1]=g;
                if(g>highVal[1])
                    highVal[1]=g;
                
                if(b<lowVal[2])
                    lowVal[2]=b;
                if(b>highVal[b])
                    highVal[b]=b;
            }
            delay(500);
        }
        
        switch(i){
            case 1:
                culoare1[0]=lowVal[0]-1;
                culoare1[1]=lowVal[1]-1;
                culoare1[2]=lowVal[2]-1;
                culoare1[3]=highVal[0]+1;
                culoare1[4]=highVal[1]+1;
                culoare1[5]=highVal[2]+1;

                
                command="";
                newAngleDown="0.2 ";
                command =cale;
                command +=pinDown;
                command +=newAngleDown;
                command +=newAngleDown;
                  
                p.runShellCommand(command);
                while(p.running());

                
                break;
            case 2:
                culoare2[0]=lowVal[0]-1;
                culoare2[1]=lowVal[1]-1;
                culoare2[2]=lowVal[2]-1;
                culoare2[3]=highVal[0]+1;
                culoare2[4]=highVal[1]+1;
                culoare2[5]=highVal[2]+1;

                command="";
                newAngleDown="0.8 ";
                command =cale;
                command +=pinDown;
                command +=newAngleDown;
                command +=newAngleDown;
                  
                p.runShellCommand(command);
                while(p.running());
                
                break;
            case 3:
                culoare3[0]=lowVal[0]-1;
                culoare3[1]=lowVal[1]-1;
                culoare3[2]=lowVal[2]-1;
                culoare3[3]=highVal[0]+1;
                culoare3[4]=highVal[1]+1;
                culoare3[5]=highVal[2]+1;

                command="";
                newAngleDown="1.5 ";
                command =cale;
                command +=pinDown;
                command +=newAngleDown;
                command +=newAngleDown;
                  
                p.runShellCommand(command);
                while(p.running());
                
                break;
            case 4:
                culoare4[0]=lowVal[0]-1;
                culoare4[1]=lowVal[1]-1;
                culoare4[2]=lowVal[2]-1;
                culoare4[3]=highVal[0]+1;
                culoare4[4]=highVal[1]+1;
                culoare4[5]=highVal[2]+1;

                command="";
                newAngleDown="2 ";
                command =cale;
                command +=pinDown;
                command +=newAngleDown;
                command +=newAngleDown;
                  
                p.runShellCommand(command);
                while(p.running());
                
                break;
        }

      command="";
      newAngleUp="0.1 ";
      command =cale;
      command +=pinUp;
      command +=newAngleUp;
      command +=newAngleUp;
        
      p.runShellCommand(command);
      while(p.running());
    
      lastAngleUp=newAngleUp;

      //Revenire servo in pozitie initiala
      command="";
      newAngleUp="1.85 ";
      command =cale;
      command +=pinUp;
      command +=newAngleUp;
      command +=lastAngleUp;
        
      p.runShellCommand(command);
      while(p.running());
    
      lastAngleUp=newAngleUp;

      delay(300);
  }
}

//functie detectie culoare
int readColor(){
  int r=readRed();
  int g=readGreen();
  int b=readBlue();

  if(r>=culoare1[0] && r<=culoare1[3])
        if(g>=culoare1[1] && g<=culoare1[4])
            if(b>=culoare1[2] && b<=culoare1[5])
                return 1;
    
    if(r>=culoare2[0] && r<=culoare2[3])
        if(g>=culoare2[1] && g<=culoare2[4])
            if(b>=culoare2[2] && b<=culoare2[5])
                return 2;
            
    if(r>=culoare3[0] && r<=culoare3[3])
        if(g>=culoare3[1] && g<=culoare3[4])
            if(b>=culoare3[2] && b<=culoare3[5])
                return 3;
            
    if(r>=culoare4[0] && r<=culoare4[3])
        if(g>=culoare4[1] && g<=culoare4[4])
            if(b>=culoare4[2] && b<=culoare4[5])
                return 4;
            
    return 0;
}

// Citire Culoare Rosu - returneaza valoarea R din RGB
int readRed() {
  
  //Setare S2 si S3 pt culoarea rosu
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  
  //Citire frecventa
  frequency = pulseIn(sensorOut, LOW);
  int R = frequency;
  
  //Output Consola
  Console.print("R= ");
  Console.print(R);
  Console.print("  ");
  delay(50);
  
  return R;
}

// Citire Culoare Verde - returneaza valoarea G din RGB
int readGreen(){

   //Setare S2 si S3 pt culoarea verde
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  
  //Citire frecventa
  frequency = pulseIn(sensorOut, LOW);
  int G = frequency;
  
  // Output Consola
  Console.print("G= ");
  Console.print(frequency);
  Console.print("  ");
  delay(50);

  return G;
}

// Citire Culoare Albastru - returneaza valoarea B din RGB
int readBlue(){
  
  //Setare S2 si S3 pt culoarea albastru
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  
  //Citire frecventa
  frequency = pulseIn(sensorOut, LOW);
  int B = frequency;
  
  //Output Consola
  Console.print("B= ");
  Console.print(frequency);
  Console.println("  ");
  delay(50);

  return B;
}