       bUpdateFlagsShared |= ROLL_FLAG;
     }
 }
-
-
-void SetServos(int byteCount)
-{
-
-// the RPI shoud send 4 x 1 uint16_t (2 bytes) values
-// one for each PID updated Motor speed
-  if (byteCount == 8)
-    {
-      while(Wire.available()) {
-
-	for (int i=0;i<SERVO_NUM;i++)
-	  {
-	    rcv_data[i].b[0] = Wire.read(); //upper bits?
-	    rcv_data[i].b[1] = Wire.read(); //upper bits?
-
-	  }
-      }
-    }
-
-  return;
-}

  //Set servo values to min
  for (int i=0;i<SERVO_NUM;i++)
    {
      MOTOR[i].writeMicroseconds(RC_MIN);
    }


  // initialize i2c as slave
  // define call backs ofr I2C


void loop()
{
  //nothing to do here
}

//on Receive
void SetServos(int byteCount)
{

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

  digitalWrite(LED_PIN,HIGH);
  if (byteCount == 8)
    {

      //Serial.println(Wire.available());

      while(Wire.available()) {

	for (int i=0;i<SERVO_NUM;i++)
	  {
	    rcv_data[i].b[0] = Wire.read(); //upper bits?
	    rcv_data[i].b[1] = Wire.read(); //lower bits?
	  }
      }
      //update servos
      for (int i=0;i<SERVO_NUM;i++)
	{
	  MOTOR[i].writeMicroseconds(rcv_data[i].i);
	}

    }

  digitalWrite(LED_PIN,LOW);

  return;
}


