mdc_client/MDCClient.cpp at main · TNCT-Mechatech/mdc_client · 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
#include "MDCClient.hpp"
#include <cstdio>

MDCClient::MDCClient(CANSerialBridge *serial, uint8_t device_id)
    :   _serial(serial), _device_id(device_id),
    PING_ID(resolve_id(device_id, MessageID::PING)),
    COMMAND_ID(resolve_id(device_id, MessageID::COMMAND)),
    ACKNOWLEDGE_ID(resolve_id(device_id, MessageID::ACKNOWLEDGE)),
    SETTING_ID(resolve_id(device_id, MessageID::SETTING)),
    TARGET_ID(resolve_id(device_id, MessageID::TARGET)),
    FEEDBACK_ID(resolve_id(device_id, MessageID::FEEDBACK))
{
    //  add frame
    serial->add_frame(PING_ID, &ping_msg);
    serial->add_frame(COMMAND_ID, &command_msg);
    serial->add_frame(ACKNOWLEDGE_ID, &acknowledge_msg);
    serial->add_frame(SETTING_ID, &setting_msg);
    serial->add_frame(TARGET_ID, &target_msg);
    serial->add_frame(FEEDBACK_ID, &feedback);
}

bool MDCClient::update() {
    bool is_received = false;

    if(ping_msg.was_updated()) {
        _serial->write(PING_ID);
    }

    if (acknowledge_msg.was_updated()) {
        //  todo remove hash

        is_received = true;
    }

    if (feedback.was_updated()) {
        is_received = true;
    }

    return is_received;
}

bool MDCClient::update_setting(uint8_t node_id, setting_struct_t settings)
{
    if (node_id > 3) {
        return false;
    }

    setting_msg.data.nodeId = node_id;
    //  copy
    setting_msg.data.mode = settings.mode;
    setting_msg.data.encoderType = settings.encoderType;
    setting_msg.data.scale = settings.scale;
    setting_msg.data.reverse = settings.reverse;
    setting_msg.data.kp = settings.kp;
    setting_msg.data.ki = settings.ki;
    setting_msg.data.cp = settings.cp;
    setting_msg.data.ci = settings.ci;
    setting_msg.data.cd = settings.cd;

    //  write
    _serial->write(SETTING_ID);
    return true;
}

bool MDCClient::set_target(uint8_t node_id, float target)
{
    if (node_id > 3) {
        return false;
    }

    //  set
    target_msg.data.target[node_id] = target;
    return true;
}

void MDCClient::send_target()
{
    //  send
    _serial->write(TARGET_ID);
}