File content as of revision 3:4a4463380739:

#include "SpeedMsg.h"

SpeedMsg::SpeedMsg () {

    counter1 = counter2 = 0;
    //c2inc = { 1, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2, 1, 2, 1, 2, 2 };
    char x = 0;
    for (int i = 0; i < 16; i++) {
        c2inc[i] = 1 + x;
        if (i != 5 && i != 14) x = 1 - x;
    }        
    c2incp = 0;
    
    spb[0] = 0; spb[1] = 0;
    spb[2] = 0; spb[3] = 0;
    spb[4] = 0; spb[5] = 0;
}

void SpeedMsg::setSpeed(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, uint8_t b5) {

    spb[0] = b0; spb[1] = b1;
    spb[2] = b2; spb[3] = b3;
    spb[4] = b4; spb[5] = b5;
}

bool SpeedMsg::sendMessage(CAN *can) {

    uint8_t data[8];
    
    data[0] = spb[0]; data[1] = spb[1];
    data[2] = spb[2]; data[3] = spb[3];
    data[4] = spb[4]; data[5] = spb[5];
    
    data[6] = counter1;
    data[7] = 0xF0 + counter2;
    
    bool ok = can->write(CANMessage(0x1A6, (char*) data, 8));

    counter1 = counter1 + 0x90;
    counter2 = (counter2 + c2inc[c2incp++]) & 0x0f;
    if (c2incp > 15) c2incp = 0;

    return ok;
}