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
|
#include "stdio.h"
#include <cmath>
#include <algorithm>
struct {
int direction = 0;
int frequency = 0;
int potValue = 1023;
} current, target;
const int frequency_max = 6000;
int newPotValue;
bool timer_enabled = false;
int x = 0;
bool loop()
{
const int step_delay = 300;
if (current.frequency != target.frequency || current.direction != target.direction)
{
if (!timer_enabled && target.frequency != 0)
{
timer_enabled = true;
fprintf(stderr, "enable motor\n");
x += step_delay;
printf("[%d, %d],\n", x, 0);
}
/*int step = (current.potValue < 20) ? 1
: (current.potValue < 100) ? 2
: (current.potValue < 300) ? 3
: (current.potValue < 400) ? 5
: (current.potValue < 500) ? 10
: 20;*/
int step = (current.frequency > 5000) ? 1
: (current.frequency > 4500) ? 2
: (current.frequency > 4000) ? 5
: (current.frequency > 3000) ? 10
: (current.frequency > 1000) ? 25
: 50;
int frequency = (current.direction == target.direction) ? target.frequency : 0;
step = std::min(abs(frequency - current.frequency), step);
if (frequency < current.frequency)
step *= -1;
current.frequency += step;
x += step_delay;
printf("[%d, %d],\n", x, current.frequency);
if (current.frequency == 0 && current.direction != target.direction)
{
timer_enabled = false;
current.direction = target.direction;
fprintf(stderr, "disable motor. changed direction to %d\n", current.direction);
x += step_delay;
printf("[%d, %d],\n", x, 0);
}
}
else if (timer_enabled && target.potValue == 0)
{
timer_enabled = false;
fprintf(stderr, "disable motor\n");
x += step_delay;
printf("[%d, %d],\n", x, 0);
}
else
return false;
return true;
}
int main(int argc, char *argv[])
{
target.frequency = frequency_max;
target.potValue = 1023;
while(loop());
return 0;
}
|
