#include <avr/sleep.h>
#include <avr/power.h>
#include <avr/wdt.h>
volatile int f_wdt=1;
#define xPin A1
#define yPin A2
#define zPin A3
#define alPin 0
#define posPowPin 1
#define timeLimit 10000
#define posCountSleep 9
#define posCountMove 231
#define posCountBack 41
#define tresholdSleep 25
#define tresholdMove 25
#define tresholdBack 20
#define movPercentMove 92
#define movPercentBack 40
#define startTimeOut 5000
#define motionTimeOut 1500
unsigned long timeOut;
byte treshold = 15;
int posCounter = 1;
byte posMode = 0;
int posTolerance = 0;
int x, y, z, x1, y1, z1;
int relX, relY, relZ;
int posCount;
boolean alarmRaised = false;
boolean standBy = false;
#define adc_disable() (ADCSRA &= ~(1<<ADEN))
#define adc_enable() (ADCSRA |= (1<<ADEN))
ISR(WDT_vect)
{
if(f_wdt == 0)
{
f_wdt=1;
}
}
void enterSleep(void)
{
pinMode(alPin, INPUT);
pinMode(posPowPin, INPUT);
adc_disable();
set_sleep_mode(SLEEP_MODE_PWR_DOWN);
sleep_enable();
sleep_mode();
sleep_disable();
power_all_enable();
adc_enable();
pinMode(alPin, OUTPUT);
digitalWrite(alPin, LOW);
pinMode(posPowPin, OUTPUT);
digitalWrite(posPowPin, LOW);
}
void setup() {
MCUSR &= ~(1<<WDRF);
WDTCR |= (1<<WDCE) | (1<<WDE);
WDTCR = 1<<WDP0 | 1<<WDP3;
WDTCR |= _BV(WDIE);
pinMode(alPin, OUTPUT);
digitalWrite(alPin, LOW);
pinMode(posPowPin, OUTPUT);
digitalWrite(posPowPin, LOW);
pinMode(xPin, INPUT);
pinMode(yPin, INPUT);
pinMode(zPin, INPUT);
delay(1000);
blinker(500, 1);
if (motionDetect(startTimeOut) == true) {
getPos();
if (abs(x1-y1) > abs(x1-z1)) {
if (abs(x1-z1) > abs(y1-z1)) {
selectX();
}
else {
posMode = 2;
posCount = posCountMove;
treshold = tresholdMove;
blinker(1000, 1);
blinker(500, 2);
}
}
else {
if (abs(x1-y1) > abs(z1-y1)) {
selectX();
}
else {
posMode = 1;
posCount = posCountSleep;
treshold = tresholdSleep;
blinker(1000, 1);
blinker(500, 1);
}
}
}
if (posMode > 0) {
getPos();
x = x1;
y = y1;
z = z1;
timeOut = millis();
while ((millis() - timeOut) < timeLimit) {
getPos();
if (comparePos(x, y, z, x1, y1, z1) == false) {
blinker(1000, 1);
x = x1;
y = y1;
z = z1;
timeOut = millis();
}
}
standBy = true;
blinker(500, 3);
}
else {
blinker(1500, 2);
}
}
void loop() {
if(f_wdt == 1) {
if (posMode == 0) {
f_wdt = 0;
enterSleep();
}
else {
getPos();
if (posCounter <= posCount && alarmRaised == false) {
if (posMode == 2) {
if (motionDetect(motionTimeOut) == true) {
posTolerance = posTolerance++;
}
if ((posCounter - posTolerance) > (posCount*movPercentMove)/100) {
alarmRaised = true;
}
} else {
if (comparePos(x, y, z, x1, y1, z1) == true) {
posTolerance = posTolerance++;
}
}
if (posMode == 1) {
if (posTolerance >= (posCount - 1)) {
alarmRaised = true;
}
}
if (posMode == 3) {
if ((posCounter - posTolerance) > (posCount*movPercentBack)/100) {
alarmRaised = true;
}
}
posCounter++;
}
else {
posCounter = 1;
posTolerance = 0;
}
if (alarmRaised == true) {
if (posMode == 1) {
blinker(500, 8);
getPos();
if (comparePos(x, y, z, x1, y1, z1) == false) {
alarmRaised = false;
posCounter = 0;
}
}
if (posMode == 2) {
blinker(500, 10);
alarmRaised = false;
posCounter = 0;
}
if (posMode == 3) {
blinker(500, 1);
getPos();
if (comparePos(x, y, z, x1, y1, z1) == true) {
alarmRaised = false;
posCounter = 0;
}
}
posTolerance = 0;
}
else {
f_wdt = 0;
enterSleep();
}
}
}
}
void blinker(unsigned int impulse, byte times) {
for (byte ttimes = 0; ttimes < times; ttimes++) {
digitalWrite(alPin, HIGH);
delay(impulse);
digitalWrite(alPin, LOW);
delay(impulse);
}
}
boolean comparePos(int xOne, int yOne, int zOne, int xTwo, int yTwo, int zTwo) {
boolean compareRes = false;
relX = xOne - xTwo;
relY = yOne - yTwo;
relZ = zOne - zTwo;
if (abs(relX) < treshold && abs(relY) < treshold && abs(relZ) < treshold) {
compareRes = true;
}
return compareRes;
}
boolean motionDetect(int detectTimeOut) {
boolean motionDetected = false;
getPos();
x = x1; y = y1; z = z1;
timeOut = millis();
while (((millis() - timeOut) < detectTimeOut)) {
if (motionDetected == false) {
if (comparePos(x, y, z, x1, y1, z1) == false) {
motionDetected = true;
}
else {
getPos();
}
}
}
return motionDetected;
}
void getPos(){
digitalWrite(posPowPin, HIGH);
delay(10);
byte i;
unsigned int posX = 0;
unsigned int posY = 0;
unsigned int posZ = 0;
for (i = 0; i < 100; i++) {
posX = posX + analogRead(xPin);
posY = posY + analogRead(yPin);
posZ = posZ + analogRead(zPin);
}
x1 = posX/100;
y1 = posY/100;
z1 = posZ/100;
digitalWrite(posPowPin, LOW);
}
void selectX() {
posMode = 3;
posCount = posCountBack;
treshold = tresholdBack;
blinker(1000, 1);
blinker(500, 3);
}