captain-balados/bala.cpp at main · keyhr/captain-balados · 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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#include "bala.h"

#include "M5Stack.h"

Bala::Bala() {
    wheel_left_encoder = 0;
    wheel_right_encoder = 0;
    i2c_mutex = NULL;
}

void Bala::ClearEncoder() {
    SetEncoder(0, 0);
    wheel_left_encoder = 0;
    wheel_right_encoder = 0;
}

void Bala::GetEncoder(int32_t* wheel_left, int32_t* wheel_right) {
    UpdateEncoder();
    *wheel_left = wheel_left_encoder;
    *wheel_right = wheel_right_encoder;
}

void Bala::SetEncoder(int32_t wheel_left, int32_t wheel_right) {
    uint8_t data_out[8];
    data_out[0] = (uint8_t)(wheel_left >> 24);
    data_out[1] = (uint8_t)(wheel_left >> 16);
    data_out[2] = (uint8_t)(wheel_left >> 8);
    data_out[3] = (uint8_t)(wheel_left >> 0);

    data_out[4] = (uint8_t)(wheel_right >> 24);
    data_out[5] = (uint8_t)(wheel_right >> 16);
    data_out[6] = (uint8_t)(wheel_right >> 8);
    data_out[7] = (uint8_t)(wheel_right >> 0);
    if (i2c_mutex != NULL) {
        xSemaphoreTake(i2c_mutex, portMAX_DELAY);
    }
    M5.I2C.writeBytes(0x3A, 0x10, data_out, 8);
    if (i2c_mutex != NULL) {
        xSemaphoreGive(i2c_mutex);
    }
}

void Bala::UpdateEncoder() {
    uint8_t data_in[8];
    if (i2c_mutex != NULL) {
        xSemaphoreTake(i2c_mutex, portMAX_DELAY);
    }
    M5.I2C.readBytes(0x3A, 0x10, 8, data_in);
    if (i2c_mutex != NULL) {
        xSemaphoreGive(i2c_mutex);
    }

    wheel_left_encoder = (data_in[0] << 24) | (data_in[1] << 16) |
                         (data_in[2] << 8) | data_in[3];
    wheel_right_encoder = (data_in[4] << 24) | (data_in[5] << 16) |
                          (data_in[6] << 8) | data_in[7];
}

void Bala::SetSpeed(int16_t wheel_left, int16_t wheel_right) {
    uint8_t data_out[4];

    data_out[0] = (int8_t)(wheel_left >> 8);
    data_out[1] = (int8_t)(wheel_left >> 0);
    data_out[2] = (int8_t)(wheel_right >> 8);
    data_out[3] = (int8_t)(wheel_right >> 0);

    if (i2c_mutex != NULL) {
        xSemaphoreTake(i2c_mutex, portMAX_DELAY);
    }
    M5.I2C.writeBytes(0x3A, 0x00, data_out, 4);
    if (i2c_mutex != NULL) {
        xSemaphoreGive(i2c_mutex);
    }
}

void Bala::SetMutex(SemaphoreHandle_t* mutex) { i2c_mutex = *mutex; }

void Bala::SetServoAngle(uint8_t pos, uint8_t angle) {
    if (pos < 1) {
        pos = 1;
    } else if (pos > 8) {
        pos = 8;
    }

    pos = pos - 1;

    if (i2c_mutex != NULL) {
        xSemaphoreTake(i2c_mutex, portMAX_DELAY);
    }
    M5.I2C.writeBytes(0x3A, 0x20 | pos, &angle, 1);
    if (i2c_mutex != NULL) {
        xSemaphoreGive(i2c_mutex);
    }
}

void Bala::SetServoPulse(uint8_t pos, uint16_t width) {
    if (pos < 1) {
        pos = 1;
    } else if (pos > 8) {
        pos = 8;
    }

    pos = (pos - 1) << 1;
    uint8_t buff_out[2];
    buff_out[0] = width >> 8;
    buff_out[1] = width & 0xff;

    if (i2c_mutex != NULL) {
        xSemaphoreTake(i2c_mutex, portMAX_DELAY);
    }
    M5.I2C.writeBytes(0x3A, 0x30 | pos, buff_out, 2);
    if (i2c_mutex != NULL) {
        xSemaphoreGive(i2c_mutex);
    }
}