To integrate the MPU6050 (or MPU6550 as mentioned) with the Arduino Leonardo and communicate with Synthiam ARC, you can use the MPU6050 library to handle the sensor data. Below is a modified version of your Arduino sketch that includes the necessary code to read data from the MPU6050 and send it to ARC.

First, ensure you have the MPU6050 library installed in your Arduino IDE. You can install it via the Library Manager by searching for "MPU6050".

Here's the updated code:

#include <Wire.h>
#include <MPU6050.h>
#include <SendOnlySoftwareSerial.h>

// The first digital port that is usable on this controller
#define _PortStart 0

// The last digital port on this controller that is usable
#define _PortCnt 14

// The number of analog ports
#define _AnalogPorts 5

// The firmware version that is reported to ARC to notify of capabilities
#define _FIRMWARE_ID 0x0000000E

// The communication baud rate
#define _BAUD_RATE 115200

// The primary communication interface between ARC and this controller
#define COMMUNICATION_PORT Serial

// The amount of RX buffer on the communication interface for EZ-Builder
#define _BUFFER_SIZE 512

// The amount of RX buffer on the Serial 1 (ttl) port
#define _BUFFER_SIZE1 512

// For EZB Comm
byte _INPUT_BUFFER[_BUFFER_SIZE];
unsigned int _WRITE_POSITION = 0;
unsigned int _READ_POSITION = 0;

// For Serial1
byte _INPUT_BUFFER1[_BUFFER_SIZE1];
unsigned int _WRITE_POSITION1 = 0;
unsigned int _READ_POSITION1 = 0;
bool _SERIAL_1_INITIALIZED = false;

// Accelerometer buffer
int _accelX = 0;
int _accelY = 0;
int _accelZ = 0;

// Gyro buffer
int _gyroX = 0;
int _gyroY = 0;
int _gyroZ = 0;

// Pose buffer
int _poseX = 0;
int _poseY = 0;
int _poseZ = 0;

long _BAUD_RATES[] = {4800, 9600, 19200, 38400, 57600, 115200, 115200};
Servo Servos[_PortCnt];

MPU6050 mpu;

void setup() {
  Wire.begin();
  COMMUNICATION_PORT.begin(_BAUD_RATE);
  mpu.initialize();
  if (!mpu.testConnection()) {
    while (1) {
      // Failed to connect to MPU6050
    }
  }
}

void loop() {
  doEZProtocol();
  updateMPUData();
}

void updateMPUData() {
  mpu.getMotion6(&_accelX, &_accelY, &_accelZ, &_gyroX, &_gyroY, &_gyroZ);
  // Convert raw data to meaningful values if needed
  // For example, convert to g's or degrees per second
}

void doEZProtocol() {
  if (!IsAvail()) return;
  byte cmd = ReadByte();
  if (cmd == CmdPing) {
    COMMUNICATION_PORT.write(222);
  } else if (cmd == CmdGetFirwareID) {
    Write32(_FIRMWARE_ID);
  } else if (cmd == CmdReleaseAllServos) {
    for (int port = _PortStart; port <= _PortCnt; port++) {
      if (Servos[port].attached()) Servos[port].detach();
    }
  } else if (cmd == CmdSetServoPosition) {
    // Handle servo position command
  } else if (cmd == CmdGetADCValue) {
    // Handle ADC value command
  } else if (cmd == CmdSendSerial) {
    // Handle serial send command
  } else if (cmd == CmdEZBv4) {
    byte cmd2 = ReadByte();
    if (cmd2 == CmdV4UARTExpansion1Read) {
      uint16_t length = Read16();
      for (uint16_t x = 0; x < length; x++) {
        switch (x) {
          case 0: COMMUNICATION_PORT.write((byte)(_accelX >> 8)); break;
          case 1: COMMUNICATION_PORT.write((byte)(_accelX & 0xff)); break;
          case 2: COMMUNICATION_PORT.write((byte)(_accelY >> 8)); break;
          case 3: COMMUNICATION_PORT.write((byte)(_accelY & 0xff)); break;
          case 4: COMMUNICATION_PORT.write((byte)(_accelZ >> 8)); break;
          case 5: COMMUNICATION_PORT.write((byte)(_accelZ & 0xff)); break;
          case 6: COMMUNICATION_PORT.write((byte)(_gyroX >> 8)); break;
          case 7: COMMUNICATION_PORT.write((byte)(_gyroX & 0xff)); break;
          case 8: COMMUNICATION_PORT.write((byte)(_gyroY >> 8)); break;
          case 9: COMMUNICATION_PORT.write((byte)(_gyroY & 0xff)); break;
          case 10: COMMUNICATION_PORT.write((byte)(_gyroZ >> 8)); break;
          case 11: COMMUNICATION_PORT.write((byte)(_gyroZ & 0xff)); break;
          default: COMMUNICATION_PORT.write((byte)0); break;
        }
      }
    }
  }
}

bool IsAvail() {
  return _WRITE_POSITION != _READ_POSITION || COMMUNICATION_PORT.available();
}

int Avail() {
  if (_READ_POSITION > _WRITE_POSITION)
    return (_BUFFER_SIZE - _READ_POSITION) + _WRITE_POSITION + COMMUNICATION_PORT.available();
  return (_WRITE_POSITION - _READ_POSITION) + COMMUNICATION_PORT.available();
}

byte ReadByte() {
  if (_READ_POSITION == _WRITE_POSITION) {
    while (!COMMUNICATION_PORT.available());
    return COMMUNICATION_PORT.read();
  }
  byte b = _INPUT_BUFFER[_READ_POSITION++];
  if (_READ_POSITION == _BUFFER_SIZE) _READ_POSITION = 0;
  return b;
}

void Write32(long val) {
  COMMUNICATION_PORT.write((byte)(val & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 16) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 24) & 0xff));
}

Key Points:

• The MPU6050 library is used to interface with the MPU6050 sensor.
• The updateMPUData() function reads the accelerometer and gyroscope data from the MPU6050.
• The doEZProtocol() function handles communication with ARC, including sending sensor data.
• The sensor data is sent as high and low bytes to ARC.

This code provides a basic framework for integrating the MPU6050 with ARC. You can expand upon this by adding more functionality or refining the data processing as needed. If you have any further questions or need additional assistance, feel free to ask!