#include #include #define DEBUG 0 #define LOWERLIMIT 400 #define UPPERLIMIT 650 #define COUNTLIMIT 20 Servo turret; Servo outServo; byte sensor_address = 0xd0; int PIRREAD[4][5]; int PIR[4]; byte index = 0; byte currentState = 4; byte currentServo = 90; void setup() { turret.attach(9); // attaches the servo on pin 9 to the servo object turret.write(90); outServo.attach(10); outServo.write(90); Wire.begin(); Serial.begin(9600); for(int i=0; i<6; i++) { PIRREAD[0][i] = analogRead(0); PIRREAD[1][i] = analogRead(1); PIRREAD[2][i] = analogRead(2); PIRREAD[3][i] = analogRead(3); } delay(1000); } void loop() { byte retVal, decision; int initVal = 90; recordPIR(); decision = 4; if(PIR[0] > UPPERLIMIT || PIR[0] < LOWERLIMIT) { if(DEBUG) Serial.println("main loop 0"); decision = stateZeroHit(0,currentState); } else if(PIR[1] > UPPERLIMIT || PIR[1] < LOWERLIMIT) { if(DEBUG) Serial.println("main loop 1"); decision = stateOneHit(1,currentState); } else if(PIR[2] > UPPERLIMIT || PIR[2] < LOWERLIMIT) { if(DEBUG) Serial.println("main loop 2"); decision = stateTwoHit(2,currentState); } else if(PIR[3] > UPPERLIMIT || PIR[3] < LOWERLIMIT) { if(DEBUG) Serial.println("main loop 3"); decision = stateThreeHit(3,currentState); } switch(decision){ case 0: initVal = 20; break; case 1: initVal = 65; break; case 2: initVal = 115; break; case 3: initVal = 160; break; } if(decision < 4) { if(DEBUG) Serial.println(PIR[decision]); if(abs(initVal-currentServo) > 5) { outServo.write(initVal); currentServo = initVal; } //outServo.write(110); retVal = recordThermo(decision); if(retVal < 180) { //c = (int)floor(sqrt(11600-8000*(cos((90+retVal)*2*PI/180)))); //beta = (int)floor((90 - acos((-8400+c*c)/80*c))); outServo.write(retVal); } currentState = decision; } delay(50); } byte recordThermo(byte sector) { byte pos; byte heat[9]; int idxHigh=0; byte valHigh=0; switch(sector){ case 0: pos = 2; break; case 1: pos = 65; break; case 2: pos = 115; break; case 3: pos = 178; break; } turret.write((int)pos); if(DEBUG) Serial.print("scanning sector "); if(DEBUG) Serial.println((int)sector); delay(500); int i=0; for(i=1; i<=9; i++) { Wire.beginTransmission(sensor_address>>1); Wire.send(i); Wire.endTransmission(); Wire.requestFrom(sensor_address>>1, (int) 1); while(Wire.available() < 1) { ; } heat[i-1] = Wire.receive(); // receive a byte as character } for(i = 1; i<9; i++) { heat[i]-=heat[0]; if(heat[i] > 100) heat[i] = 0; if(heat[i] > valHigh) { idxHigh = i; valHigh = heat[i]; } if(DEBUG) Serial.print(heat[i], DEC); if(DEBUG) Serial.print(" "); } if(DEBUG) Serial.println(""); if(valHigh > 3) { return pos - 5*(idxHigh-4); } return 200; } byte stateZeroHit(byte detected, byte lastState) { if(DEBUG) Serial.println("hit state 0"); byte count = 0; if(lastState != 1) { while(count < COUNTLIMIT && detected == 0) { recordPIR(); if(PIR[1] > UPPERLIMIT || PIR[1] < LOWERLIMIT) { detected = 1; detected = stateOneHit(detected, 0); } count++; } } return detected; } byte stateOneHit(byte detected, byte lastState) { if(DEBUG) Serial.println("hit state 1"); byte count = 0; if(lastState == 0) { while(count < COUNTLIMIT && detected == 1) { recordPIR(); if(PIR[2] > UPPERLIMIT || PIR[2] < LOWERLIMIT) { detected = 2; detected = stateTwoHit(detected, 1); } count++; } } else if(lastState == 2) { while(count < COUNTLIMIT && detected == 1) { if(PIR[0] > UPPERLIMIT || PIR[0] < LOWERLIMIT) { detected = 0; detected = stateZeroHit(detected, 1); } else recordPIR(); count++; } } return detected; } byte stateTwoHit(byte detected, byte lastState) { if(DEBUG) Serial.println("hit state 2"); byte count = 0; if(lastState == 1) { while(count UPPERLIMIT || PIR[3] < LOWERLIMIT) { detected = 3; detected = stateThreeHit(detected, 2); } count++; } } else if(lastState == 3) { while(count < COUNTLIMIT && detected == 2) { recordPIR(); if(PIR[1] > UPPERLIMIT || PIR[1] < LOWERLIMIT) { detected = 1; detected = stateTwoHit(detected, 2); } count++; } } return detected; } byte stateThreeHit(byte detected, byte lastState) { if(DEBUG) Serial.println("hit state 3"); byte count = 0; if(lastState != 2) { while(count < COUNTLIMIT && detected == 3) { recordPIR(); if(PIR[2] > UPPERLIMIT || PIR[2] < LOWERLIMIT) { detected = 2; detected = stateTwoHit(detected, 3); } count++; } } return detected; } void recordPIR() { int count, currentval; PIRREAD[0][index] = analogRead(0); delay(5); PIRREAD[1][index] = analogRead(1); delay(5); PIRREAD[2][index] = analogRead(2); delay(5); PIRREAD[3][index] = analogRead(3); //for each sensor for (int i = 0; i<4; i++) { //for each index in the sensor for(int m = 0; m<5; m++) { currentval = PIRREAD[i][m]; count = 0; for(int q = 0; q<5; q++) { if(currentval >= PIRREAD[i][q]) count++; } if (count == 3) { PIR[i]=PIRREAD[i][m]; m=20; } } } index++; if(index >=5) index = 0; }