Robot Arm Color Sorting

#include<Servo.h>
#include<LiquidCrystal.h>
LiquidCrystal lcd={A0,A1,A2,A3,A4,A5};
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
#define S0 8
#define S1 9
#define S2 10
#define S3 11
#define sensorOut 12
int frequency = 0;
int color=0;
void setup() 
{
 //lcd.setCursor(6,0);
 myservo1.attach(4);
 myservo2.attach(3);
 myservo3.attach(14);
 myservo4.attach(15);
  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);
  Serial.begin(9600);
  lcd.begin(16,2);
}

void loop() 
{
 
    myservo1.write(85);
    myservo2.write(70);
    myservo3.write(80);
    myservo4.write(100);
      color = readColor();
      delay(10);
    switch (color) {
    case 1:
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Color : ");
    lcd.print("Red");
    for(int i=70;i>=30;i--)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=80,j=100;i<=100,j>=75;i++,j--)
   {
    myservo3.write(i);
    myservo4.write(j);
    delay(30);
   }
   for(int i=30;i<=70;i++)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=85;i>=0;i--)
   {
    myservo1.write(i);
    delay(30); 
   }
   for(int i=70;i>=40;i--)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=100,j=75;i>=80,j<=100;i--,j++)
   {
    myservo3.write(i);
    myservo4.write(j);
    delay(30);
   }
   for(int i=40;i<=70;i++)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=0;i<=85;i++)
   {
    myservo1.write(i);
    delay(30); 
   };
    break;
    case 2:
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Color : ");
    lcd.print("Green");
    for(int i=70;i>=30;i--)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=80,j=100;i<=100,j>=75;i++,j--)
   {
    myservo3.write(i);
    myservo4.write(j);
    delay(30);
   }
   for(int i=30;i<=70;i++)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=85;i>=25;i--)
   {
    myservo1.write(i);
    delay(30); 
   }
   for(int i=70;i>=40;i--)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=100,j=75;i>=80,j<=100;i--,j++)
   {
    myservo3.write(i);
    myservo4.write(j);
    delay(30);
   }
   for(int i=40;i<=70;i++)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=20;i<=85;i++)
   {
    myservo1.write(i);
    delay(30); 
   };
    break;
    case 3:
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Color : ");
    lcd.print("Blue");
      for(int i=70;i>=30;i--)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=80,j=100;i<=100,j>=75;i++,j--)
   {
    myservo3.write(i);
    myservo4.write(j);
    delay(30);
   }
   for(int i=30;i<=70;i++)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=85;i>=50;i--)
   {
    myservo1.write(i);
    delay(30); 
   }
   for(int i=70;i>=40;i--)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=100,j=75;i>=80,j<=100;i--,j++)
   {
    myservo3.write(i);
    myservo4.write(j);
    delay(30);
   }
   for(int i=40;i<=70;i++)
   {
    myservo2.write(i);
    delay(30); 
   }
   for(int i=40;i<=85;i++)
   {
    myservo1.write(i);
    delay(30); 
   };
    break;  
    case 4:
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("   No  Object"); 
    delay(1000);break;
    
    case 0:
    break;
    }
    delay(300);
    color=0;
  
}
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<130 & R>100 & G<195 & G>160){
    color = 1; // Red
  }
  if(G<160 & G>135 & B<140 & B>110){//
    color = 2; // green
  }
  if(R<175 & R>145 & G<200 & G>180){//
    color = 3; // Blue
  }
  if(G<250 & G>200 & B<180 & B>150){
    color = 4; // no object
  }
 /* if(R<56 & R>46 & G<65 & G>55){
    color = 5; // Brown
  }
  if (G<58 & G>45 & B<40 &B>26){
    color = 6; // Blue
  }*/
  return color;  
}