code

#include <Servo.h> // Library for servos
#include <Pixy2.h> // Libraries for PIXYCAM
#include <SPI.h>  


Pixy2 pixy; // This is the main Pixy object 
Servo myservo1; // Servo for RED
Servo myservo2; // Servo for BLUE
const int trigPin = 5;
const int echoPin = 6;
long duration;
int distance;
int pos = 0; //stores servo position
void setup(){
  myservo1.attach(9); // Attach servo for RED
  myservo1.write(180);
  myservo2.attach(3); // Attach servo for BLUE
  myservo2.write(180);
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  Serial.begin(115200);
  pixy.init(); // Initiate PIXYCAM
}

void loop(){ 
  int i; 
  pixy.ccc.getBlocks(); // grab blocks!
  if (pixy.ccc.numBlocks)// If there are detect blocks, print them!
  {
    Serial.print("Detected ");
    Serial.println(pixy.ccc.numBlocks);
    for (i=0; i<pixy.ccc.numBlocks; i++)
    digitalWrite(trigPin, LOW); // Clears the trigPin
    delayMicroseconds(2); // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW); // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH); // Calculating the distance
    distance= duration*0.034/2; // Converts distance
    {
      Serial.print("  block ");
      Serial.print(i);
      Serial.print(": ");
      pixy.ccc.blocks[i].print(); // Prints PIXYCAM values
      Serial.print("Distance: "); // Prints the distance on the Serial Monitor
      Serial.println(distance);
    }
      if (uint16_t m_signature == 2 && (distance < 25)){ // if color signature is RED and distance is less than the value denoted
      {
        delay(1000);
        myservo1.write(0);
        delay(2500);
        myservo1.write(90);
        delay(500);
      }
      }
      if (uint16_t m_signature == 1 && (distance < 25)){ // if color signature is BLUE and distance is less than the value denoted
      {
        delay(1000);
        myservo2.write(0);
        delay(2500);
        myservo2.write(90);
        delay(500);
      }
      }
  }
      }