Maplebot/dxl_gy85.cpp at master · RhobanProject/Maplebot · 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
116
117
118
119
120
121
122
123
124
#include <stdlib.h>
#include <wirish/wirish.h>
#include <string.h>
#include "dxl_gy85.h"
#include "dxl_protocol.h"
#include "coder.h"
#include "gy85.h"

#define GY85_ADDR       0x24
#define BUFFERS 5

struct gy85
{
    i2c_dev *dev;

    // State
    // 0-3: updating devices
    // 4: waiting
    int state;

    // Last update timestamp
    int lastUpdate;

    // Values buffer
    struct gy85_value values[BUFFERS];
    uint32_t sequence;
    int pos;

    // Registers
    struct dxl_registers registers;
};

void gy85_begin(struct gy85 *gy85, i2c_dev *dev)
{
    gy85->sequence = 0;
    gy85->pos = 0;
    gy85->dev = dev;
    gy85->lastUpdate = millis();

    gy85_init(dev);
}

void gy85_tick(struct gy85 *gy85)
{
    int now = millis();
    int delta = now-gy85->lastUpdate;

    if (gy85->state < 3) {
        gy85_update(gy85->dev, &gy85->values[gy85->pos], gy85->state);
        gy85->state++;
        if (gy85->state >= 3) {
            gy85->sequence++;
            gy85->values[gy85->pos].sequence = gy85->sequence;
            gy85->pos++;
            if (gy85->pos >= BUFFERS) {
                gy85->pos = 0;
            }
        }
    } else {
        if (delta >= 10) {
            gy85->lastUpdate += 10;
            gy85->state = 0;
        } else if (delta < 0) {
            gy85->lastUpdate = millis();
        }
    }
};


static bool dxl_gy85_check_id(volatile struct dxl_device *self, ui8 id)
{
    struct gy85 *gy85 = (struct gy85*)self->data;
    return (id == gy85->registers.eeprom.id);
}

static void dxl_gy85_write_data(volatile struct dxl_device *self, ui8 id, ui8 addr, ui8 *values, ui8 length)
{
    struct gy85 *gy85 = (struct gy85*)self->data;
    memcpy(((ui8 *)(&gy85->registers))+addr, values, length);
}

static void dxl_gy85_read_data(volatile struct dxl_device *self, ui8 id, ui8 addr, ui8 *values, ui8 length, ui8 *error)
{
    struct gy85 *gy85 = (struct gy85*)self->data;

    if (addr == GY85_ADDR) {
        memcpy(values, ((ui8*)&gy85->values), length);
    } else {
        memcpy(values, ((ui8*)&gy85->registers)+addr, length);
    }

    *error = 0;
}

static void dxl_gy85_process(volatile struct dxl_device *self, volatile struct dxl_packet *packet)
{
    dxl_process(self, packet, dxl_gy85_check_id, dxl_gy85_write_data, dxl_gy85_read_data);
}

static void gy85_tick(volatile struct dxl_device *self)
{
    struct gy85 *gy85 = (struct gy85*)self->data;
    gy85_tick(gy85);
}

void dxl_gy85_init(volatile struct dxl_device *device, ui8 id, i2c_dev *dev)
{
    struct gy85 *gy85 = (struct gy85 *)malloc(sizeof(struct gy85));
    dxl_device_init(device);
    device->process = dxl_gy85_process;
    device->data = (void *)gy85;
    device->tick = gy85_tick;

    gy85_begin(gy85, dev);

    gy85->state = 4;
    gy85->registers.eeprom.modelNumber = DXL_GY85_MODEL;
    gy85->registers.eeprom.firmwareVersion = 1;
    gy85->registers.eeprom.id = id;
    gy85->registers.ram.presentPosition = 0;
    gy85->registers.ram.goalPosition = 0;
    gy85->registers.ram.torqueEnable = 0;
    gy85->registers.ram.torqueLimit = 0;
}