Diff: JrkG2.cpp

Revision:

0:33bfb28b0ffc

Child:

2:e78c0ddcf337

--- /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