#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);
}