I was making an "Air defense system" using arduino uno. The idea is the have two servos (vertical and horizontal) attached to an Ultrasonic sensor and called "radar" which will rotate and scan the area, then calculate the position of the closest object it found and send this to rotate a "laser" which also has two servos to move. It's supposed to use some trigonometry and vector math to calculate both the position of object and the angles the "laser" has to move both upwards and sideways.
This is what the "radar" looks like (don't worry, I'll have it decorated):
You can see the sensor attached to a servo on side (it'll move it upwards), and another servo at the bottom (it'll move it sideways), and when the vertical angle is 0, it points straight ahead, and currently the horizontal angle is around 60 degrees, i.e., 0=point left-ish (towards arduino) and 120 = point right-ish (away from arduino)
I had this code:
#include <Servo.h>
const int pinRadarServoV = 2; // V=vertical (i.e. upwards rotating servo)
const int pinRadarServoH = 3; // H=horizontal (i.e. sideways rotating servo)
const int pinRadarSensorTrig = 9;
const int pinRadarSensorEcho = 10;
Servo radarServoV;
Servo radarServoH;
const int radarX = 0; // X
const int radarY = 7; // Y
const int radarZ = 0; // and Z positions of the radar's center
const float pi = 3.14159;
int adderV = 2; // it will alternate between 2 and -2
int adderH = 2; // both to be added to H and V angles per loop.
int angleV = 0;
int angleH = 0;
float minDistance = 0; // save the minimum distance per revolution
int minDistAngleH = 0; // and have it H
int minDistAngleV = 0; // and V angles stored
// Use the Ultrasonic sensor.
int radarSensorDistance() {
digitalWrite(pinRadarSensorTrig, LOW);
delayMicroseconds(2);
digitalWrite(pinRadarSensorTrig, HIGH);
delayMicroseconds(10);
digitalWrite(pinRadarSensorTrig, LOW);
long duration = pulseIn(pinRadarSensorEcho, HIGH);
float distance = (duration * 0.0343) / 2;
return distance;
}
// the function that needs help.
void triggerAction() {
if (minDistance>0) {
int X = radarX + minDistance * cos(angleV*(pi/180)) * cos(angleH*(pi/180));
int Y = radarY + minDistance * cos(angleV*(pi/180)) * sin(angleH*(pi/180));
int Z = radarZ + minDistance * sin(angleV*(pi/180));
Serial.print("object (");
Serial.print(X);
Serial.print(",");
Serial.print(Y);
Serial.print(",");
Serial.print(Z);
Serial.print(") at d:");
Serial.print(minDistance);
Serial.print(", angleH: ");
Serial.print(minDistAngleH);
Serial.print(", and angleV: ");
Serial.println(minDistAngleV);
minDistance = 0.0f;
minDistAngleH = 0;
minDistAngleV = 0;
}
}
void setup() {
Serial.begin(9600);
radarServoV.attach(pinRadarServoV);
radarServoH.attach(pinRadarServoH);
pinMode(pinRadarSensorTrig, OUTPUT);
pinMode(pinRadarSensorEcho, INPUT);
radarServoV.write(0); // reset both servos
radarServoH.write(0); // for good measures
delay(1000);
}
void loop() {
if (angleV>=75) {angleV=75; adderV=-2;} // limit it to 75 upwards and 120 sideways
if (angleH>=120) {angleH=120; adderH=-2;} // and go back to 0 on both.
if (angleV<=0) {angleV=0; adderV=+2; triggerAction();} // trigger that function
if (angleH<=0) {angleH=0; adderH=+2; triggerAction();} // on reaching 0
radarServoV.write(angleV);
radarServoH.write(angleH);
delay(15);
float distanceNow = 0; // temporary variable to store Sensor data.
if (angleH%20==0 || angleV%15==0) distanceNow = radarSensorDistance();
if (distanceNow>0 && distanceNow<20) { // work only if distance is less than 20cm.
if (minDistance==0) minDistance = distanceNow; // setting first time.
if (distanceNow<=minDistance) {
// updated global variables if current detection has least distance.
distanceNow = minDistance;
minDistAngleH = angleH;
minDistAngleV = angleV;
}
}
angleV+=adderV;
angleH+=adderH;
}
Here, upon running it, I get awkward result, such as:
It's awkward because, in many of these readings, the Z value turns out to be 0 which isn't possible because the object can't be "on" the sensor.
lastly, I want to have an accurate way to calculate the point of object if the "radar"'s sensor's center is at (radarX, radarY, radarZ), and also calculate angles the "laser" has to point to, if its center is at, say, (p,q,r).
Thanks 
... also, do you mean that my equations are valid or those are wrong?
