File content as of revision 13:6a4969d25627:

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

#define MAXTRY 1
#define SetDWORDval(arg) (uint8_t)(arg>>24),(uint8_t)(arg>>16),(uint8_t)(arg>>8),(uint8_t)arg
#define SetWORDval(arg) (uint8_t)(arg>>8),(uint8_t)arg

RoboClaw::RoboClaw(uint8_t adr, int baudrate, PinName rx, PinName tx, double _read_timeout) : _roboclaw(rx, tx){
    _roboclaw.set_baud(baudrate);//_roboclaw.baud(baudrate);
       
    rx_in = 0;
    address = adr;
    //readTime = 0.0;
    //readTimer.start();
    read_timeout = _read_timeout;
    
}

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


void RoboClaw::flushSerialBuffer(void) { 
    char char1[1];
     
    while (_roboclaw.readable()) { 
        _roboclaw.read_timeout(char1,1,read_timeout); 
    } 
return; }

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

uint16_t RoboClaw::crc_get(){
    return crc;
}

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

void RoboClaw::write_n(uint8_t cnt, ... ){
    uint8_t count = 0;
    char dat[1];
    char r_dat[1];
    r_dat[0] = ' ';
    char crc_[2];
    do {
        _roboclaw.flush();

        crc_clear();
        va_list marker;
        va_start( marker, cnt );
        for(uint8_t index=0; index<cnt; index++) {
            uint8_t data = va_arg(marker, unsigned int);
            dat[0] = data;
            crc_update(data);
            _roboclaw.write(dat,1);
                   }
        va_end( marker );
        uint16_t crc = crc_get();
        crc_[0]=crc>>8;
        crc_[1]=crc;
        _roboclaw.write(crc_,2);        
        _roboclaw.read_timeout(r_dat,1,read_timeout);
        
         count ++;
         
        }while((uint16_t)r_dat[0] != 0xFF&& count <=MAXTRY);

}

void RoboClaw::write_(uint8_t command, uint8_t data, bool reading, bool crcon){
   // _roboclaw.putc(address);
   // _roboclaw.putc(command);

    if(reading == false) {
        if(crcon == true) {
            uint8_t packet[2] = {address, command};
            uint16_t checksum = crc16(packet,2);
           // _roboclaw.putc(checksum>>8);
           // _roboclaw.putc(checksum);
        } else {
            uint8_t packet[3] = {address, command, data};
            uint16_t checksum = crc16(packet,3);
           // _roboclaw.putc(data);
           // _roboclaw.putc(checksum>>8);
           // _roboclaw.putc(checksum);
        }
    }
}

bool RoboClaw::read_n(uint8_t address, uint8_t cmd, int n_byte, int32_t &value){
    uint16_t read_byte[n_byte];
    uint16_t received_crc;
    int dataRead=0;
    char tx_buffer[2];
    
    tx_buffer[0] = address;
    tx_buffer[1] = cmd;    

    rx_in = 0;    
    crc_clear();

    _roboclaw.flush();
    _roboclaw.write(tx_buffer,2);
    
    crc_update(address);
    crc_update(cmd);
    
    dataRead = _roboclaw.read_timeout(rx_buffer,7,read_timeout);
    //printf("d: %d\r\n", dataRead);
    //printf("add: %x,   cmd: %x,   0: %x,   1: %x,   2: %x,   3: %x,   4: %x,   5: %x,   6: %x \r\n",(uint16_t)tx_buffer[0],(uint16_t)tx_buffer[1],(uint16_t)rx_buffer[0],(uint16_t)rx_buffer[1],(uint16_t)rx_buffer[2],(uint16_t)rx_buffer[3],(uint16_t)rx_buffer[4],(uint16_t)rx_buffer[5],(uint16_t)rx_buffer[6]); 
    //printf("dopo del read\r\n");       
    for(int i = 0; i<=n_byte-3; i++){
        crc_update((uint16_t)rx_buffer[i]);
    }
    
    received_crc = (uint16_t)rx_buffer[n_byte-2]<<8;
    received_crc |= (uint16_t)rx_buffer[n_byte-1];
    
    if(received_crc == crc_get()){
        value = (uint16_t)rx_buffer[0]<<24;
        value |= (uint16_t)rx_buffer[1]<<16;
        value |= (uint16_t)rx_buffer[2]<<8;
        value |= (uint16_t)rx_buffer[3];
        return true;
    }else{
        //printf("crc: %x \r\n", crc_get());
        //printf("received crc: %x \r\n", received_crc);
        //printf("err \r\n");


        return false;        
    }
}


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

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

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

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

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

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

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

bool RoboClaw::ReadEncM1(int32_t &encPulse){
    int32_t value=0;
    if (read_n(address, GETM1ENC, 7, value) == true){
        encPulse = value;
        return true;
    }else{
        return false;
    }
}

bool RoboClaw::ReadEncM2(int32_t &encPulse){
    int32_t value=0;
    if (read_n(address, GETM2ENC, 7, value) == true){
        encPulse = value;
        return true;
    }else{
        return false;
    }
}

bool RoboClaw::ReadSpeedM1(int32_t &speed){
    int32_t value=0;
    if (read_n(address, GETM1SPEED, 7, value) == true){
        speed = value;
        return true;
    }else{
        return false;
    }
}

bool RoboClaw::ReadSpeedM2(int32_t &speed){
    int32_t value=0;
    if (read_n(address, GETM2SPEED, 7, value) == true){
        speed = value;
        return true;
    }else{
        return false;
    }
}


bool RoboClaw::ReadCurrentM1M2(int32_t& currentM1, int32_t& currentM2){
    int32_t value=0;
    if (read_n(address, GETCURRENTS, 6, value) == true){
        currentM1 = value>>16;
        currentM2 = value&0xFFFF;
        return true;
    }else{
        return false;
    }

}

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

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

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

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

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

void RoboClaw::SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2){
    write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2));
}

void RoboClaw::SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer){
    write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer){
    write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedAccelDistanceM1(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
    write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedAccelDistanceM2(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
    write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
}

void RoboClaw::SpeedAccelDeccelPositionM1(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
    write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
}

void RoboClaw::SpeedAccelDeccelPositionM2(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
    write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
}

void RoboClaw::SpeedAccelDeccelPositionM1M2(uint32_t accel1,uint32_t speed1,uint32_t deccel1, int32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2, int32_t position2,uint8_t flag){
    write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
}