Library for JrkG2. this bases on Arduino Library (https://github.com/pololu/jrk-g2-arduino)
Diff: JrkG2.cpp
- Revision:
- 0:33bfb28b0ffc
- Child:
- 2:e78c0ddcf337
diff -r 000000000000 -r 33bfb28b0ffc JrkG2.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/JrkG2.cpp Sat Aug 25 13:00:25 2018 +0000 @@ -0,0 +1,214 @@ +#include <JrkG2.h> + +/**** JrkG2Serial ****/ + +void JrkG2Serial::commandW7(uint8_t cmd, uint8_t val) +{ + sendCommandHeader(cmd); + serialW7(val); + + _lastError = 0; +} + +void JrkG2Serial::commandWs14(uint8_t cmd, int16_t val) +{ + uint16_t v = val; + sendCommandHeader(cmd); + serialW7(v); // lower 7 bits + serialW7(v >> 7); // upper 7 bits + + _lastError = 0; +} + +uint8_t JrkG2Serial::commandR8(uint8_t cmd) +{ + uint8_t val; + + sendCommandHeader(cmd); + if(_stream->readable() != 1) + { + _lastError = JrkG2CommReadError; + return 0; + } + val = _stream->getc(); + _lastError = 0; + return val; +} + +uint16_t JrkG2Serial::commandR16(uint8_t cmd) +{ + uint8_t buffer[2]; + + sendCommandHeader(cmd); + + //if(_stream->ByteCount() != 2) //バッファリング未実装のため不可 + // → BufferedSerialを使用すべきか, タイムアウトも実装したい + + //これ以降も同様の理由で修正される + if(_stream->readable() == 0) + { + _lastError = JrkG2CommReadError; + return 0; + } + for(int i = 0; i < 2; i++) + buffer[i] = _stream->getc(); + + _lastError = 0; + return ((uint16_t)buffer[0] << 0) | ((uint16_t)buffer[1] << 8); +} + +void JrkG2Serial::segmentRead(uint8_t cmd, uint8_t offset, + uint8_t length, uint8_t * buffer) +{ + // The Jrk does not allow reads longer than 15 bytes. + if (length > 15) { length = 15; } + + sendCommandHeader(cmd); + serialW7(offset); + serialW7(length); + + //if(_stream->ByteCount() != length) + if(_stream->readable() != 1) + { + _lastError = JrkG2CommReadError; + // Set the buffer bytes to 0 so the program will not use an uninitialized + // variable. + memset(buffer, 0, length); + return; + } + for(int i = 0; i < length; i++) + buffer[i] = _stream->getc(); + + _lastError = 0; +} + +void JrkG2Serial::segmentWrite(uint8_t cmd, uint8_t offset, + uint8_t length, uint8_t * buffer) +{ + // The Jrk does not accept writes longer than 7 bytes over serial. + if (length > 7) { length = 7; } + + sendCommandHeader(cmd); + serialW7(offset); + serialW7(length); + + // bit i = most-significant bit of buffer[i] + uint8_t msbs = 0; + for (uint8_t i = 0; i < length; i++) + { + serialW7(buffer[i]); + msbs |= (buffer[i] >> 7 & 1) << i; + } + serialW7(msbs); + + _lastError = 0; +} + +void JrkG2Serial::sendCommandHeader(uint8_t cmd) +{ + if (_deviceNumber == 255) + { + // Compact protocol + _stream->putc((uint8_t)cmd); + } + else + { + // Pololu protocol + _stream->putc(0xAA); + serialW7(_deviceNumber); + serialW7((uint8_t)cmd); + } + _lastError = 0; +} + +/**** JrkG2I2C ****/ + +void JrkG2I2C::commandQuick(uint8_t cmd) +{ + char data[2] = {cmd, 0}; + _lastError = _i2c->write(_address, data, 1); +} + +void JrkG2I2C::commandW7(uint8_t cmd, uint8_t val) +{ + char data[2] = {cmd, val & 0x7F}; + _lastError = _i2c->write(_address, data, 2); +} + +void JrkG2I2C::commandWs14(uint8_t cmd, int16_t val) +{ + uint16_t v = val; + char data[3] = {cmd, v & 0xFF, v >> 8 & 0xFF}; + _lastError = _i2c->write(_address, data, 3); +} + +uint8_t JrkG2I2C::commandR8(uint8_t cmd) +{ + char w_data[2] = {cmd, 0}; + _lastError = _i2c->write(_address, w_data, 1); + if (_lastError != 0) { return 0; } + + char r_data; + uint8_t val = _i2c->read(_address, &r_data, 1); + if (val != 0) + { + _lastError = JrkG2CommReadError; + return 0; + } + _lastError = 0; + return r_data; +} + +uint16_t JrkG2I2C::commandR16(uint8_t cmd) +{ + char w_data[2] = {cmd, 0}; + _lastError = _i2c->write(_address, w_data, 1, true); + if (_lastError != 0) { return 0; } + + char r_data[2] = {}; + uint8_t val = _i2c->read(_address, r_data, 2); + if (val != 0) + { + _lastError = JrkG2CommReadError; + return 0; + } + _lastError = 0; + uint8_t valL = r_data[0]; + uint8_t valH = r_data[1]; + return (uint16_t)valL | ((uint16_t)valH << 8); +} + +void JrkG2I2C::segmentRead(uint8_t cmd, uint8_t offset, + uint8_t length, uint8_t * buffer) +{ + // The Jrk does not allow reads longer than 15 bytes. + if (length > 15) { length = 15; } + + char data[2] = {cmd, offset}; + _lastError = _i2c->write(_address, data, 2, true); + if (_lastError != 0) + { + memset(buffer, 0, length); + return; + } + + uint8_t val = _i2c->read(_address, (char*)buffer, length); + if (val != 0) + { + _lastError = JrkG2CommReadError; + memset(buffer, 0, length); + return; + } + _lastError = 0; +} + +void JrkG2I2C::segmentWrite(uint8_t cmd, uint8_t offset, + uint8_t length, uint8_t * buffer) +{ + // The Jrk does not accept writes longer than 13 bytes over I2C. + if (length > 13) { length = 13; } + + char data[3] = {cmd, offset, length}; + _lastError = _i2c->write(_address, data, 3); + _i2c->write(_address, (char*)buffer, length); +} \ No newline at end of file