include scanlines include Arduino define H_STEP define V_STEP 10 defin

 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
#include "scanlines.h"
#include "Arduino.h"
#define H_STEP 1
#define V_STEP 10
#define H_WIDE_SENSOR_OFFSET 5
#define V_WIDE_SENSOR_OFFSET 5
namespace Scanlines
{
static HorizontalDirection hDirection = DIRECTION_RIGHT;
static VerticalDirection vDirection = DIRECTION_UP;
void init(short& horizontalAngle, short& verticalAngle)
{
horizontalAngle = 0;
verticalAngle = 0;
hDirection = DIRECTION_RIGHT;
vDirection = DIRECTION_UP;
}
VerticalDirection reverse(VerticalDirection dir)
{
return (dir == DIRECTION_UP) ? DIRECTION_DOWN : DIRECTION_UP;
}
HorizontalDirection reverse(HorizontalDirection dir)
{
return (dir == DIRECTION_LEFT) ? DIRECTION_RIGHT : DIRECTION_LEFT;
}
inline void angleStep(HorizontalDirection dir, short& angle)
{
angle += (dir == DIRECTION_LEFT) ? (-H_STEP) : (H_STEP);
}
inline void angleStep(VerticalDirection dir, short& angle)
{
angle += (dir == DIRECTION_DOWN) ? (-V_STEP) : (V_STEP);
}
void calculate(short& horizontalAngle, short& verticalAngle, WideSensorData& wideSensorData, Direction& dir, HorizontalDirection& hDir, VerticalDirection& vDir)
{
angleStep(hDirection, horizontalAngle);
dir = DIRECTION_HORIZONTAL;
if (horizontalAngle >= min(180, (wideSensorData.right + H_WIDE_SENSOR_OFFSET)) || horizontalAngle <= max(0, wideSensorData.left - H_WIDE_SENSOR_OFFSET))
{
hDirection = reverse(hDirection);
angleStep(vDirection, verticalAngle);
if (verticalAngle >= min(180, (wideSensorData.up + V_WIDE_SENSOR_OFFSET)) || verticalAngle <= max(0, wideSensorData.down - V_WIDE_SENSOR_OFFSET))
{
dir = DIRECTION_BOTH;
vDirection = reverse(vDirection);
}
}
hDir = hDirection;
vDir = vDirection;
}
}