initial commit

libraries/FirmataWithDeviceFeature/src-features/Encoder7Bit.cpp

@@ -0,0 +1,64 @@
+/*
+  Encoder7Bit.cpp - Firmata library
+  Copyright (C) 2012-2013 Norbert Truchsess. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#include "Encoder7Bit.h"
+#include <ConfigurableFirmata.h>
+
+Encoder7BitClass::Encoder7BitClass()
+{
+  previous = 0;
+  shift = 0;
+}
+
+void Encoder7BitClass::startBinaryWrite()
+{
+  shift = 0;
+}
+
+void Encoder7BitClass::endBinaryWrite()
+{
+  if (shift > 0) {
+    Firmata.write(previous);
+  }
+}
+
+void Encoder7BitClass::writeBinary(byte data)
+{
+  if (shift == 0) {
+    Firmata.write(data & 0x7f);
+    shift++;
+    previous = data >> 7;
+  }
+  else {
+    Firmata.write(((data << shift) & 0x7f) | previous);
+    if (shift == 6) {
+      Firmata.write(data >> 1);
+      shift = 0;
+    }
+    else {
+      shift++;
+      previous = data >> (8 - shift);
+    }
+  }
+}
+
+void Encoder7BitClass::readBinary(int outBytes, byte *inData, byte *outData)
+{
+  for (int i = 0; i < outBytes; i++) {
+    int j = i << 3;
+    int pos = j / 7;
+    byte shift = j % 7;
+    outData[i] = (inData[pos] >> shift) | ((inData[pos + 1] << (7 - shift)) & 0xFF);
+  }
+}
+
+Encoder7BitClass Encoder7Bit;