File content as of revision 2:e78c0ddcf337:

#include <JrkG2.h>

#define serial_delay_us 1500    //baud:9600

/**** 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);
    wait_us(serial_delay_us);
    if(_stream->readable() != 1)
    {
        _stream->abort_read();
        _lastError = JrkG2CommReadError;
        return 0;
    }
    val = _stream->getc();
    _lastError = 0;
    return val;
}

uint16_t JrkG2Serial::commandR16(uint8_t cmd)
{
    uint8_t buffer[2];
    
    sendCommandHeader(cmd);
    
    wait_us(serial_delay_us);
    if(_stream->readable() != 2)
    {
        _stream->abort_read();
        _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);
    wait_us(serial_delay_us);
    serialW7(length);
    wait_us(serial_delay_us);
    if(_stream->readable() != length)
    {
        _stream->abort_read();
        _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);
}