void setup enum State STATE_RIGHT STATE_DOWN STATE_LEFT STATE_UP volat

 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
void setup()
{
}
enum State
{
STATE_RIGHT,
STATE_DOWN,
STATE_LEFT,
STATE_UP
};
volatile int horizontalAngle = 0;
volatile int verticalAngle = 180;
State state = STATE_RIGHT;
int delta = 0;
void loop()
{
static int hMs = 0, vMs = 0;
PORTB = (1 << PB3) | (1 << PB4);
if (hMs < vMs)
{
delayMicroseconds(hMs);
PORTB = (1 << PB3);
delayMicroseconds(vMs);
PORTB = 0;
}
else
{
delayMicroseconds(vMs);
PORTB = (1 << PB4);
delayMicroseconds(hMs);
PORTB = 0;
}
/* digitalWrite(PB3, LOW);
digitalWrite(PB4, HIGH);
delayMicroseconds(map(verticalAngle, 0, 180, 750, 2250));
digitalWrite(PB4, LOW);
*/
// offCounter1 = toCounter(horizontalAngle);
// offCounter2 = toCounter(verticalAngle);
if (state == STATE_RIGHT)
{
++horizontalAngle;
if (horizontalAngle >= (180 - delta))
{
state = STATE_DOWN;
}
}
else if (state == STATE_LEFT)
{
--horizontalAngle;
if (horizontalAngle <= delta)
{
state = STATE_UP;
}
++delta;
}
else if (state == STATE_UP)
{
++verticalAngle;
if (verticalAngle >= (180 - delta))
{
state = STATE_RIGHT;
}
}
else if (state == STATE_DOWN)
{
--verticalAngle;
if (verticalAngle <= delta)
{
state = STATE_LEFT;
}
}
if (delta >= 90)
{
delta = 0;
}
hMs = map(horizontalAngle, 0, 180, 800, 2100);
vMs = map(verticalAngle, 0, 180, 800, 2100);
delayMicroseconds(15000);
}