Fixed bugs, made general implementation using Wiring for Energia/Arduino cross-compatibility