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
|
#include "project.h"
static unsigned motor_pos[HANDS];
static void coils (unsigned m, int a, int b, int c, int d)
{
printf ("Motor %d: %+d %+d\r\n", m, a - b, c - d);
}
static void step (unsigned m, int d)
{
unsigned s = motor_pos[m];
if (d < 0) d += MOTOR_STEPS;
s += d;
s %= MOTOR_STEPS;
coils (m, !! ((s + 6) & 4), !! ((s + 1) & 4), !! ((s + 4) & 4), !! ((s + 7) & 4));
if (!m)
BKP_DR8 = motor_pos[0] = s;
else
BKP_DR9 = motor_pos[1] = s;
}
void motor_tick (void)
{
static uint32_t last;
unsigned i, d;
if ((ticks - last) < 10) return;
last = ticks;
if (!hands_ready) return;
//printf("HANDS: %d -> %d %d -> %d\r\n",hands_pos[0],motor_pos[0],hands_pos[1],motor_pos[1]);
for (i = 0; i < HANDS; ++i) {
d = MOTOR_STEPS + hands_pos[i];
d -= motor_pos[i];
d %= MOTOR_STEPS;
if (d) {
if (d < (MOTOR_STEPS / 2))
step (i, 1);
else
step (i, -1);
}
}
}
void motor_init (void)
{
motor_pos[0] = BKP_DR8;
motor_pos[1] = BKP_DR9;
step (0, 0);
step (1, 0);
}
|
