65 lines
1.4 KiB
C++
65 lines
1.4 KiB
C++
/*
|
|
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;
|