added read6reg emulator function, so we can test without the robovero. Still left to do: test this function and change the imu class angle calculation implementations. They are very wrong now due to the cleaning up of the gyro and accel classes