#include #define trigPin 13 // sonic sensor trig to pin 13 #define echoPin 12 // sonic sensor echo to pin 12 const int led01 = 5; // led 1 to pin 5 const int led02 = 6; // led 2 to pin 6 Servo MyServo; int pos = 0; // current position of the rotor int trig = 1; int lastPos =0; //saves the last position of the rotor int ledBrightness = 255; //current brightness of the leds int lastDistance = 0; //last measured distance int lastBrightness = 255; // saves the last brightness void setup() { pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); pinMode(led01, OUTPUT); pinMode(led02, OUTPUT); Serial.begin(9600); MyServo.attach(11); // servo to pin 11 } void loop() { byte ledBrightness; long duration, distance; digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance = (duration/2) / 29.1; lastDistance = distance; //10 steps of 20cm a step // each step adds 5 degrees to the servo if (distance >= 160 and distance < 180) { pos = 45; lastPos = 45; lastBrightness = 180; MyServo.write(pos); ledBrightness = 180; analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance >= 140 and distance < 160) { pos = 40; lastPos = 40; lastBrightness = 160; ledBrightness = 160; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance >= 120 and distance < 140) { pos = 35; lastPos = 35; lastBrightness = 140; ledBrightness = 140; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance >= 120 and distance < 140) { pos = 30; lastPos = 30; lastBrightness = 120; ledBrightness = 120; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance >= 100 and distance < 120) { pos = 25; lastPos = 25; lastBrightness = 100; ledBrightness = 100; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance >= 80 and distance < 100) { pos = 20; lastPos = 20; lastBrightness = 80; ledBrightness = 80; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance >= 60 and distance < 80) { pos = 15; lastPos = 15; lastBrightness = 60; ledBrightness = 60; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if(distance > 40 and distance < 60) { pos = 10; lastPos = 10; lastBrightness = 40; ledBrightness = 40; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance <= 40 and distance > 20) { pos = 5; lastPos = 5; lastBrightness = 20; ledBrightness = 20; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else if (distance >= 0 and distance < 20) { pos = 0; lastPos = 0; lastBrightness = 0; ledBrightness = 0; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); Serial.print(ledBrightness); Serial.print(" , "); } else { pos = lastPos; ledBrightness = lastBrightness; MyServo.write(pos); analogWrite(led01, ledBrightness); analogWrite(led02, ledBrightness); } if (distance >= 200 || distance <= 0){ pos = lastPos; Serial.println(lastDistance); Serial.println("Out of range"); } else { Serial.print(distance); Serial.println(" cm"); } delay(500); }