   else {
     //short read from master
     //Do nothing
-    Serial.println("short read from master");
-    Serial.print("Expected 8 bytes");
-    Serial.print("Received");
-    Serial.print(byteCount);
-    Serial.print("bytes");
+    // Serial.println("Short read from master :");
+    // Serial.print("Expected 8 bytes,");
+    // Serial.print("Received ");
+    // Serial.print(byteCount);
+    // Serial.println(" bytes");
+
+    return;
   }
 
   //Update servo values
-  FL_MOTOR.writeMicroseconds(b[0]);
-  FR_MOTOR.writeMicroseconds(b[1]);
-  BL_MOTOR.writeMicroseconds(b[2]);
-  BR_MOTOR.writeMicroseconds(b[3]);
+  for (int i=0;i<SERVO_NUM;i++)
+    {
+      	    // Serial.print("Received value ");
+	    // Serial.print(i);
+	    // Serial.print(" = ");
+	    // Serial.println(rcv_data[i].i);
 
+      MOTOR[i].writeMicroseconds(rcv_data[i].i);
+    }
+
+  return;
 
 }
 

// the RPI shoud send 4 x 1 uint16_t (2 bytes) values
// one for each PID updated Motor speed

  //Expected bytes to be received
  uint16_t b[4];

  if (byteCount == 8)
    {
      while(Wire.available()) {
	
	for (int i=0;i<4;i++)
	  {
	    b[i]  = Wire.read() << 8; //lower bits
	    b[i] |= Wire.read();      //upper bits
	    
	    Serial.print("Received value ");
	    Serial.print(i);
	    Serial.print(" = ");
	    Serial.println(b[i]);
	  }
      }
	

#define BR_MOTOR_OUT_PIN 7

//define Servo variables
Servo MOTOR[SERVO_NUM];

void setup()
{
