radar.ino (1372B)
1 #include <LiquidCrystal.h> 2 #include <Servo.h> 3 4 #define PIN_SERVO_X 3 5 #define PIN_SERVO_Y 4 6 #define PIN_ULTRASONIC_ECHO 11 7 #define PIN_ULTRASONIC_TRIGGER 12 8 9 LiquidCrystal lcd(10, 9, 8, 7, 6, 5); 10 Servo servo_x, servo_y; 11 12 void 13 setup() 14 { 15 pinMode(PIN_ULTRASONIC_ECHO, INPUT); 16 pinMode(PIN_ULTRASONIC_TRIGGER, OUTPUT); 17 servo_x.attach(PIN_SERVO_X); 18 servo_y.attach(PIN_SERVO_Y); 19 servo_x.write(0); 20 servo_y.write(0); 21 lcd.begin(16, 2); 22 Serial.begin(9600); 23 } 24 25 void 26 loop() 27 { 28 int x, y; 29 30 /* 31 * in reality, the ultrasonic sensor is supposed to be mounted on the 2 32 * servos so that it is able to move around. 33 */ 34 for (x = 0; x < 180; x++) 35 for (y = 0; y < 180; y++) 36 move_and_calc_distance(x, y); 37 for (x = 180; x >= 0; x--) 38 for (y = 180; y >= 0; y--) 39 move_and_calc_distance(x, y); 40 41 delay(250); 42 } 43 44 void 45 move_and_calc_distance(int x, int y) 46 { 47 float pingtime, distance; 48 49 servo_x.write(x); 50 servo_y.write(y); 51 delay(10); 52 53 digitalWrite(PIN_ULTRASONIC_TRIGGER, LOW); 54 delayMicroseconds(2); 55 digitalWrite(PIN_ULTRASONIC_TRIGGER, HIGH); 56 delayMicroseconds(10); 57 digitalWrite(PIN_ULTRASONIC_TRIGGER, LOW); 58 59 pingtime = pulseIn(PIN_ULTRASONIC_ECHO, HIGH); 60 /* calculate distance in cm */ 61 distance = (float)pingtime * 0.344 / 20; 62 63 lcd.clear(); 64 lcd.setCursor(0, 0); 65 lcd.print("Distance: "); 66 lcd.setCursor(0, 1); 67 lcd.print(distance); 68 lcd.print(" cm"); 69 delay(50); 70 }