#include <Adafruit_NeoPixel.h>
#include <Servo.h>
const int neopixelPin = 2, numPixel = 1;
Adafruit_NeoPixel NeoPixel(numPixel, neopixelPin, NEO_GRB + NEO_KHZ800);
Servo servo;
const int servoPin = 8;
long duration1, duration2; int distance1, distance2; double space;
const int trig1 = 4, echo1 = 5, trig2 = 6, echo2 = 7;
const int camera = 3;
#define PI 3.1415926535897932384626433832795
int maxMistake = 10, maxSpeed = 7, car, carr;
unsigned long long time1;
void setup() {
pinMode(trig1, OUTPUT);
pinMode(trig2, OUTPUT);
pinMode(camera, OUTPUT);
pinMode(echo1, INPUT);
pinMode(echo2, INPUT);
servo.attach(servoPin);
servo.write(0);
NeoPixel.begin();
brightness(96);
blue(1);
show();
delay(100);
digitalWrite(trig1, LOW);
delayMicroseconds(2);
digitalWrite(trig1, HIGH);
delayMicroseconds(8);
digitalWrite(trig1, LOW);
duration1 = pulseIn(echo1, HIGH);
distance1 = duration1*0.017175;
double servoPoint = atan(100.00/distance1)*180/PI;
servo.write(servoPoint);
digitalWrite(trig2, LOW);
delayMicroseconds(2);
digitalWrite(trig2, HIGH);
delayMicroseconds(8);
digitalWrite(trig2, LOW);
duration2 = pulseIn(echo2, HIGH);
distance2 = duration2*0.017175;
space = sqrt(sq(distance2) - sq(distance1)) + servoPoint/3.0 + int(servoPoint*100)%100/6.0;
Serial.begin(115200);
}
void loop() {
if (distance2 >= 399) {
red(1);
show();
return;
}
green(1);
show();
digitalWrite(trig1, LOW);
delayMicroseconds(2);
digitalWrite(trig1, HIGH);
delayMicroseconds(8);
digitalWrite(trig1, LOW);
duration1 = pulseIn(echo1, HIGH);
int index1 = duration1*0.017175;
digitalWrite(trig2, LOW);
delayMicroseconds(2);
digitalWrite(trig2, HIGH);
delayMicroseconds(8);
digitalWrite(trig2, LOW);
duration2 = pulseIn(echo2, HIGH);
int index2 = duration2*0.017175;
if (index1 < distance1 - maxMistake) {
carr = 1;
} else {
carr = 0;
}
if (index2 < distance2 - maxMistake && carr == 0) {
car = 1;
time1 = micros();
}
if ((car == 1) && (index1 < distance1 - maxMistake)) {
double speed = space/(micros() - time1)*0.00036;
if (speed > maxSpeed && micros() - time1 >= 50) {
digitalWrite(camera, HIGH);
yellow(1);
show();
delay(1000);
digitalWrite(camera, LOW);
green(1);
show();
}
Serial.println(speed);
car = 0;
}
}
void red(int rd) {
NeoPixel.setPixelColor(rd-1, 227, 12, 12);
}
void blue(int bl) {
NeoPixel.setPixelColor(bl-1, 16, 69, 230);
}
void green(int gr) {
NeoPixel.setPixelColor(gr-1, 62, 232, 10);
}
void yellow(int yl) {
NeoPixel.setPixelColor(yl-1, 247, 244, 3);
}
void show() {
NeoPixel.show();
}
void brightness(int b) {
b = map(b, 0, 100, 0, 255);
NeoPixel.setBrightness(b);
}