File content as of revision 0:af5cf35e1a25:

#include "RoboClaw.h"
#include <stdarg.h>

#define SetDWORDval(arg) (unsigned char)(arg>>24),(unsigned char)(arg>>16),(unsigned char)(arg>>8),(unsigned char)arg
#define SetWORDval(arg) (unsigned char)(arg>>8),(unsigned char)arg

RoboClaw::RoboClaw(int baudrate, PinName rx, PinName tx) : _roboclaw(rx, tx)
{
    _roboclaw.baud(baudrate);
}

void RoboClaw::crc_clear()
{
    crc = 0;
}

void RoboClaw::crc_update (unsigned char data)
{
    int i;
    crc = crc ^ ((unsigned int)data << 8);
    for (i=0; i<8; i++)
    {
        if (crc & 0x8000)
            crc = (crc << 1) ^ 0x1021;
        else
            crc <<= 1;
    }
}

unsigned int RoboClaw::crc_get()
{
    return crc;
}

void RoboClaw::write_n(unsigned char cnt, ... )
{
    crc_clear();
    va_list marker;
    va_start( marker, cnt );
    for(unsigned char index=0;index<cnt;index++)
    {
        unsigned char data = va_arg(marker, unsigned int);
        crc_update(data);
        _roboclaw.putc(data);
    }
    va_end( marker );
    unsigned int crc = crc_get();
    _roboclaw.putc(crc>>8);
    _roboclaw.putc(crc);
}

void RoboClaw::write_(unsigned char address, unsigned char command, unsigned char data, bool reading, bool crcon)
{
    _roboclaw.putc(address);
    _roboclaw.putc(command);
    
    if(reading == false)
    {
        if(crcon == true)
        {
            unsigned char packet[2] = {address, command};
            unsigned int checksum = crc16(packet,2);
            _roboclaw.putc(checksum>>8);
            _roboclaw.putc(checksum);
        }
        else
        {
            unsigned char packet[3] = {address, command, data};
            unsigned int checksum = crc16(packet,3);
            _roboclaw.putc(data);
            _roboclaw.putc(checksum>>8);
            _roboclaw.putc(checksum);
        }
    }
}

unsigned int RoboClaw::crc16(unsigned char *packet, int nBytes) {
    unsigned int crc_;
    for (int byte = 0; byte < nBytes; byte++) {
        crc_ = crc_ ^ ((unsigned int)packet[byte] << 8);
        for (unsigned char bit = 0; bit < 8; bit++) {
            if (crc_ & 0x8000) {
                crc_ = (crc_ << 1) ^ 0x1021;
            } else {
                crc_ = crc_ << 1;
            }
        }
    }
    return crc_;
}

unsigned char RoboClaw::read_(void)
{
    return(_roboclaw.getc());
}

void RoboClaw::ForwardM1(unsigned char address, int speed){
    write_(address,M1FORWARD,speed,false,false);
}

void RoboClaw::BackwardM1(unsigned char address, int speed){
    write_(address,M1BACKWARD,speed,false,false);
}

void RoboClaw::ForwardM2(unsigned char address, int speed){
    write_(address,M2FORWARD,speed,false,false);
}

void RoboClaw::BackwardM2(unsigned char address, int speed){
    write_(address,M2BACKWARD,speed,false,false);
}

void RoboClaw::Forward(unsigned char address, int speed){
    write_(address,MIXEDFORWARD,speed,false,false);
}

void RoboClaw::Backward(unsigned char address, int speed){
    write_(address,MIXEDBACKWARD,speed,false,false);
}

void RoboClaw::ReadFirm(unsigned char address){
    write_(address,GETVERSION,0x00,true,false);
}

long RoboClaw::ReadEncM1(unsigned char address){
    long enc1;
    unsigned int read_byte[7];
    write_n(2,address,GETM1ENC);

    read_byte[0] = (unsigned int)_roboclaw.getc();
    read_byte[1] = (unsigned int)_roboclaw.getc();
    read_byte[2] = (unsigned int)_roboclaw.getc();
    read_byte[3] = (unsigned int)_roboclaw.getc();
    read_byte[4] = (unsigned int)_roboclaw.getc();
    read_byte[5] = (unsigned int)_roboclaw.getc();
    read_byte[6] = (unsigned int)_roboclaw.getc();
    
    enc1 = read_byte[1]<<24;
    enc1 |= read_byte[2]<<16;
    enc1 |= read_byte[3]<<8;
    enc1 |= read_byte[4];
    
    return enc1;
}

long RoboClaw::ReadEncM2(unsigned char address){
    long enc2;
    unsigned int read_byte2[7];
    write_(address,GETM2ENC,0x00, true,false);

    read_byte2[0] = (unsigned int)_roboclaw.getc();
    read_byte2[1] = (unsigned int)_roboclaw.getc();
    read_byte2[2] = (unsigned int)_roboclaw.getc();
    read_byte2[3] = (unsigned int)_roboclaw.getc();
    read_byte2[4] = (unsigned int)_roboclaw.getc();
    read_byte2[5] = (unsigned int)_roboclaw.getc();
    read_byte2[6] = (unsigned int)_roboclaw.getc();
    
    enc2 = read_byte2[1]<<24;
    enc2 |= read_byte2[2]<<16;
    enc2 |= read_byte2[3]<<8;
    enc2 |= read_byte2[4];
    
    return enc2;
}

long RoboClaw::ReadSpeedM1(unsigned char address){
    long speed1;
    unsigned int read_byte[7];
    write_n(2,address,GETM1SPEED);

    read_byte[0] = (unsigned int)_roboclaw.getc();
    read_byte[1] = (unsigned int)_roboclaw.getc();
    read_byte[2] = (unsigned int)_roboclaw.getc();
    read_byte[3] = (unsigned int)_roboclaw.getc();
    read_byte[4] = (unsigned int)_roboclaw.getc();
    read_byte[5] = (unsigned int)_roboclaw.getc();
    read_byte[6] = (unsigned int)_roboclaw.getc();
    
    speed1 = read_byte[1]<<24;
    speed1 |= read_byte[2]<<16;
    speed1 |= read_byte[3]<<8;
    speed1 |= read_byte[4];
    
    return speed1;
}

long RoboClaw::ReadSpeedM2(unsigned char address){
    long speed2;
    unsigned int read_byte2[7];
    write_n(2,address,GETM2SPEED);

    read_byte2[0] = (unsigned int)_roboclaw.getc();
    read_byte2[1] = (unsigned int)_roboclaw.getc();
    read_byte2[2] = (unsigned int)_roboclaw.getc();
    read_byte2[3] = (unsigned int)_roboclaw.getc();
    read_byte2[4] = (unsigned int)_roboclaw.getc();
    read_byte2[5] = (unsigned int)_roboclaw.getc();
    read_byte2[6] = (unsigned int)_roboclaw.getc();
    
    speed2 = read_byte2[1]<<24;
    speed2 |= read_byte2[2]<<16;
    speed2 |= read_byte2[3]<<8;
    speed2 |= read_byte2[4];
    
    return speed2;
}

void RoboClaw::ResetEnc(unsigned char address){
    write_n(2,address,RESETENC);
}

void RoboClaw::SpeedM1(unsigned char address, long speed){
    write_n(6,address,M1SPEED,SetDWORDval(speed));
}

void RoboClaw::SpeedM2(unsigned char address, long speed){
    write_n(6,address,M2SPEED,SetDWORDval(speed));
}

void RoboClaw::SpeedAccelM1(unsigned char address, long accel, long speed){
    write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
}

void RoboClaw::SpeedAccelM2(unsigned char address, long accel, long speed){
    write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
}

void RoboClaw::SpeedDistanceM1(unsigned char address, long speed, unsigned long distance, unsigned char buffer){
    write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedDistanceM2(unsigned char address, long speed, unsigned long distance, unsigned char buffer){
    write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedAccelDistanceM1(unsigned char address, long accel, long speed, unsigned long distance, unsigned char buffer){
    write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedAccelDistanceM2(unsigned char address, long accel, long speed, unsigned long distance, unsigned char buffer){
    write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedAccelDeccelPositionM1(unsigned char address, unsigned long accel, long speed, unsigned long deccel, unsigned long position, unsigned char flag){
    return write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
}

void RoboClaw::SpeedAccelDeccelPositionM2(unsigned char address, unsigned long accel, long speed, unsigned long deccel, unsigned long position, unsigned char flag){
    return write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
}