-
Notifications
You must be signed in to change notification settings - Fork 0
/
AS5600.cpp
52 lines (43 loc) · 864 Bytes
/
AS5600.cpp
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
#include "mbed.h"
#include "AS5600.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#define SLAVE_ADRESS 0x36
AS5600::AS5600(PinName i2c_sda, PinName i2c_scl):
i2c(i2c_sda, i2c_scl), angle0(0)
{
}
void AS5600::updateAngle()
{
char cmd[1];
char out[2];
cmd[0] = 0x0E;
error |= i2c.write(SLAVE_ADRESS << 1, cmd, 1);
error |= i2c.read(SLAVE_ADRESS << 1, out, 2);
angle = ((out[0] << 8) + out[1]) * 0.087912087f * M_PI / 180.0f - angle0;
while (angle > M_PI) angle -= 2.0f * M_PI;
while (angle < -M_PI) angle += 2.0f * M_PI;
}
float AS5600::getAngleRad()
{
return angle;
}
float AS5600::getAngleDeg()
{
return angle / M_PI * 180.0;
}
int AS5600::getError()
{
return error;
}
void AS5600::write(float value)
{
updateAngle();
angle0 = (angle + angle0) - value;
}
float AS5600::read()
{
updateAngle();
return angle;
}