libucomm/main.cpp at master · xqms/libucomm · 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
80
81
82
83
84
85
86
87
88
89
// Test

#include "test.h"
#include "envelope.h"
#include "checksum.h"
#include <stdio.h>

const int BUFSIZE = 1024;

class DebugCharIO : public uc::CharWriter
{
public:
    DebugCharIO()
     : m_readIdx(0)
     , m_writeIdx(0)
    {
    }

    bool writeChar(uint8_t c)
    {
        m_buffer[m_writeIdx++] = c;
        // FIXME: Range check
        return true;
    }

    bool isDataAvailable() const
    {
        return m_writeIdx != m_readIdx;
    }

    uint8_t getc()
    {
        // FIXME: Range check
        return m_buffer[m_readIdx++];
    }
private:
    uint8_t m_buffer[BUFSIZE];
    uint8_t m_readIdx;
    uint8_t m_writeIdx;
};

typedef uc::EnvelopeWriter<uc::InvertedModSumGenerator> EnvelopeWriter;

typedef uc::IO<EnvelopeWriter, uc::IO_W> SimpleWriter;
typedef Proto<SimpleWriter> WProto;

typedef uc::EnvelopeReader<uc::InvertedModSumGenerator, 1024> EnvelopeReader;
typedef uc::IO<EnvelopeReader, uc::IO_R> SimpleReader;
typedef Proto<SimpleReader> RProto;

template<class SizeType>
bool fillServoCommands(WProto::ServoCommand* cmd, SizeType idx)
{
    cmd->id = idx;
    cmd->command = 2 * idx;

    return true;
}

int main()
{
    WProto::ServoPacket pkt;
    pkt.flags = 0;
    pkt.cmds.setCallback(fillServoCommands, 4);

    DebugCharIO dbg;
    EnvelopeWriter output(&dbg);
    output << pkt << pkt;

    EnvelopeReader input;
    while(dbg.isDataAvailable())
    {
        if(input.take(dbg.getc()))
        {
            if(input.msgCode() == RProto::ServoPacket::MSG_CODE)
            {
                RProto::ServoPacket pkt2;
                input >> pkt2;

                printf("Flags: %d\n", pkt2.flags);
                RProto::ServoCommand cmd;
                while(pkt2.cmds.next(&cmd))
                {
                    printf("cmd: %d -> %d\n", cmd.id, cmd.command);
                }
            }
        }
    }
}