RobocupSSL_LL/JoystickDriver/packer.cpp at develop · jmorissette/RobocupSSL_LL · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
//version 1.0
#include "packer.h"

Packer::Packer(Serial * pSerial){
    mSerial = pSerial;
}

unsigned char Packer::FLAGBYTE = 0x7E;
unsigned char Packer::ESCAPEBYTE = 0x7D;

void Packer::createSpeedCommand(float iX, float iY, float iTheta, unsigned char iId){
    std::queue<unsigned char> emptyQueue;
    std::swap( mPacket, emptyQueue);

    this->insertFlagByteInPacket();
    mPacket.push(iId);
    mPacket.push(char(1));
    this->insertFloatInPacket(iX);
    this->insertFloatInPacket(iY);
    this->insertFloatInPacket(iTheta);
    this->insertFlagByteInPacket();
}

void Packer::createSetPidCommand(float iP, float iI, float iD, unsigned char iId){
}

void Packer::sendPacket(){
    int lSize = this->mPacket.size();
    char lBuffer[lSize + 1];
    for(int i = 0; i < lSize; ++i){
        lBuffer[i] = mPacket.front();
        mPacket.pop();
    }
    if(mSerial->IsConnected()){
        mSerial->WriteData(lBuffer,lSize + 1);
    }
    else{
        cout << "Serial not connected" << endl;
    }

}


void Packer::insertFloatInPacket(float iData){
    mDataConverter.floatValue = iData;
    for(int i = 0; i < 4; ++i){
        if(mDataConverter.charValues[i] == this->FLAGBYTE || mDataConverter.charValues[i] == this->ESCAPEBYTE){
            mPacket.push(this->ESCAPEBYTE);
        }
        mPacket.push(mDataConverter.charValues[i]);
    }
}


void Packer::insertIntInPacket(int iData){
    mDataConverter.intValue = iData;
    for(int i = 0; i < 4; ++i){
        if(mDataConverter.charValues[i] == this->FLAGBYTE || mDataConverter.charValues[i] == this->ESCAPEBYTE){
            mPacket.push(this->ESCAPEBYTE);
        }
        mPacket.push(mDataConverter.charValues[i]);
    }
}

void Packer::insertFlagByteInPacket(){
    mPacket.push(this->FLAGBYTE);
}

void Packer::printPacket(){
    std::queue<unsigned char> lCopy(mPacket);
    int lSize = lCopy.size();
    std::cout << "Printing Packet of length : " << lSize << std::endl;
    std::cout << "---------------------------------------------- " << std::endl;
    for(int i = 0; i < lSize; ++i){
        printf("%x ",lCopy.front());
        lCopy.pop();
    }
    std::cout << std::endl << "---------------------------------------------- " << std::endl;
}