define PULSE_MIN 380 define PULSE_MAX 1140 define PULSE_MIN_H PULSE_MI

  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
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
#define PULSE_MIN 380
#define PULSE_MAX 1140
#define PULSE_MIN_H (PULSE_MIN * 1.1)
#define PULSE_MAX_H (PULSE_MAX * 0.9)
#define PULSE_MIN_V (PULSE_MIN * 1.3)
#define PULSE_MAX_V (PULSE_MAX * 0.7)
#define WIDE_TIMEOUT 1000
int __count(char* string, ...)
{
int res = 1;
va_list ap;
va_start(ap, string);
va_arg(ap,void*);
res += 1;
va_end(ap);
return res;
}
#define DBG1(a) Serial.println(a)
#define DBG2(a,b) Serial.print(a); Serial.println(b);
#define DBG3(a,b,c) Serial.print(a); Serial.print(b), Serial.println(c);
#define DBG4(a,b,c,d) Serial.print(a); Serial.print(b); Serial.print(c); Serial.println(d)
#define DBG5(a,b,c,d,e) Serial.print(a); Serial.print(b); Serial.print(c); Serial.print(d); Serial.println(e)
#define DBG6(a,b,c,d,e,f) Serial.print(a); Serial.print(b); Serial.print(c); Serial.print(d); Serial.print(e); Serial.println(f)
short horizontalAngle;
short verticalAngle;
inline void setVerticalAngle(byte angle)
{
OCR1B = map(angle, 0, 180, PULSE_MIN_V, PULSE_MAX_V);
}
inline void setHorizontalAngle(byte angle)
{
OCR1A = map(angle, 0, 180, PULSE_MIN_H, PULSE_MAX_H);
}
#include "scanlines.h"
void setup()
{
Serial.begin(9600);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
DDRB = _BV(PORTB1) | _BV(PORTB2);
TCCR1A = _BV(COM1A1) | _BV(COM1B1);
TCCR1B = _BV(WGM13) | _BV(CS11);
OCR1A = (PULSE_MIN_V + PULSE_MAX_V) / 2;
OCR1B = (PULSE_MIN_H + PULSE_MAX_H) / 2;
ICR1 = 10000;
TCNT1H = 0;
TCNT1L = 0;
Scanlines::init(horizontalAngle, verticalAngle);
}
void loop()
{
static bool firstCycle = true;
static Direction generalDirection;
static VerticalDirection verticalDirection;
static HorizontalDirection horizontalDirection;
static int sensor = 0;
static int wideSensor = 0;
static int wideSensorPrev = 0;
sensor = analogRead(A1);
wideSensorPrev = wideSensor;
wideSensor = analogRead(A0);
analogWrite(7, map(wideSensor, 0, 1024, 254, 0));
analogWrite(6, map(sensor, 0, 1024, 254, 0));
static WideSensorData wideSensorData;
static unsigned int lastUpdateUp = millis();
static unsigned int lastUpdateDown = millis();
static unsigned int lastUpdateLeft = millis();
static unsigned int lastUpdateRight = millis();
unsigned int time = millis();
if (wideSensor > 600)
{
if (wideSensorData.up != 180 && time - lastUpdateUp > WIDE_TIMEOUT)
{
Serial.println("UP timed out");
wideSensorData.up = 180;
}
if (wideSensorData.down != 0 && time - lastUpdateDown > WIDE_TIMEOUT)
{
Serial.println("DOWN timed out");
wideSensorData.down = 0;
}
if (wideSensorData.left != 0 && time - lastUpdateLeft > WIDE_TIMEOUT)
{
Serial.println("LEFT timed out");
wideSensorData.left = 0;
}
if (wideSensorData.right != 180 && time - lastUpdateRight > WIDE_TIMEOUT)
{
Serial.println("RIGHT timed out");
wideSensorData.right = 180;
}
}
if (sensor < 600)
{
Serial.println("Got sensor signal, calling wideSensorData.reset()");
wideSensorData.reset();
return;
}
if (!firstCycle)
{
if (wideSensor < max(0, (wideSensorPrev - 200)))
{
DBG4("wideSensor ", wideSensor, ", prev ", wideSensorPrev);
if (generalDirection == DIRECTION_HORIZONTAL || generalDirection == DIRECTION_BOTH)
{
if (horizontalDirection == DIRECTION_RIGHT)
{
DBG2("Better wide signal moving RIGHT, setting wideSensorData.left to ", horizontalAngle);
wideSensorData.left = horizontalAngle;
lastUpdateLeft = time;
}
else if (horizontalDirection == DIRECTION_LEFT)
{
DBG2("Better wide signal moving LEFT, setting wideSensorData.right to ", horizontalAngle);
wideSensorData.right = horizontalAngle;
lastUpdateRight = time;
}
}
else if (generalDirection == DIRECTION_VERTICAL || generalDirection == DIRECTION_BOTH)
{
if (verticalDirection == DIRECTION_UP)
{
DBG2("Better wide signal moving UP, setting wideSensorData.down to ", verticalAngle);
wideSensorData.down = verticalAngle;
lastUpdateDown = time;
}
else if (verticalDirection == DIRECTION_DOWN)
{
DBG2("Better wide signal moving DOWN, setting wideSensorData.up to ", verticalAngle);
wideSensorData.up = verticalAngle;
lastUpdateUp = time;
}
}
}
if (wideSensor > wideSensorPrev + 200)
{
DBG4("wideSensor ", wideSensor, ", prev ", wideSensorPrev);
if (generalDirection == DIRECTION_HORIZONTAL || generalDirection == DIRECTION_BOTH)
{
if (horizontalDirection == DIRECTION_RIGHT && horizontalAngle < wideSensorData.right)
{
DBG2("Worse wide signal moving RIGHT, setting wideSensorData.right to ", horizontalAngle);
wideSensorData.right = horizontalAngle;
lastUpdateRight = time;
}
if (horizontalDirection == DIRECTION_LEFT && horizontalAngle > wideSensorData.left)
{
DBG2("Worse wide signal moving LEFT, setting wideSensorData.left to ", horizontalAngle);
wideSensorData.left = horizontalAngle;
lastUpdateLeft = time;
}
}
else if (generalDirection == DIRECTION_VERTICAL || generalDirection == DIRECTION_BOTH)
{
if (verticalDirection == DIRECTION_UP && verticalAngle > wideSensorData.up)
{
DBG2("Worse wide signal moving UP, setting wideSensorData.up to ", verticalAngle);
wideSensorData.up = verticalAngle;
lastUpdateUp = time;
}
if (verticalDirection == DIRECTION_DOWN && verticalAngle < wideSensorData.down)
{
DBG2("Worse wide signal moving DOWN, setting wideSensorData.down to ", verticalAngle);
wideSensorData.down = horizontalAngle;
lastUpdateDown = time;
}
}
}
}
else
{
wideSensorPrev = wideSensor;
}
Scanlines::calculate(horizontalAngle, verticalAngle, wideSensorData, generalDirection, horizontalDirection, verticalDirection);
setVerticalAngle(verticalAngle);
setHorizontalAngle(horizontalAngle);
firstCycle = false;
delay(3);
}