Metabot/firmware/src/leds.cpp at master · MultipedRobotics/Metabot · 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
#include <stdio.h>
#include "leds.h"
#ifdef RHOCK
#include <rhock/stream.h>
#endif
#ifndef __EMSCRIPTEN__
#include <dxl.h>
#endif

static char leds[12];
static bool leds_custom_flag;

static int led_value_to_dxl(int val)
{
    char dxlv = 0;
    if (val & LED_R) dxlv |= 1;
    if (val & LED_G) dxlv |= 2;
    if (val & LED_B) dxlv |= 4;
    return dxlv;
}

bool leds_are_custom()
{
    return leds_custom_flag;
}

void leds_decustom()
{
    leds_custom_flag = false;
}

void led_set(int index, int value, bool custom)
{
    if (custom) {
        leds_custom_flag = true;
    }
    leds[index-1] = value;
#ifndef __EMSCRIPTEN__
    dxl_write_byte(index, DXL_LED, led_value_to_dxl(value));
#endif
}

void led_set_all(int value, bool custom)
{
    if (custom) {
        leds_custom_flag = true;
    }
    for (unsigned int i=0; i<sizeof(leds); i++) {
        leds[i] = value;
    }
#ifndef __EMSCRIPTEN__
    dxl_write_byte(DXL_BROADCAST, DXL_LED, led_value_to_dxl(value));
#endif
}

void led_stream_state()
{
#ifdef RHOCK
    for (unsigned int i=0; i<sizeof(leds);) {
        uint8_t v = 0;
        v += leds[i++]<<4;
        v += leds[i++];
        rhock_stream_append(v);
    }
#endif
}