Найти положение объекта, если он задан в градусах вверх и в сторону от точки ⇐ C++

[Anonymous]

Я делал «Систему ПВО» на arduino uno. Идея состоит в том, чтобы иметь два сервопривода (вертикальный и горизонтальный), прикрепленные к ультразвуковому датчику и называемые «радарами», которые будут вращать и сканировать область, затем вычислять положение ближайшего найденного объекта и отправлять его для вращения «лазера», который также имеет два сервопривода для перемещения. Предполагается использовать некоторую тригонометрию и векторную математику для расчета положения объекта и углов, под которыми «лазер» должен двигаться вверх и в стороны.
Вот как выглядит «радар». например (не волнуйтесь, я его украшу):


Вы можете видеть датчик, прикрепленный к сервоприводу сбоку (он перемещает его вверх), и другой сервопривод внизу (он перемещает его в сторону), и когда вертикальный угол равен 0, он указывает прямо вперед, а горизонтальный угол в настоящее время составляет около 60 градусов, т.е. 0 = точка влево (в сторону Arduino) и 120 = точка вправо (от Arduino)
У меня был такой код:

Код: Выделить всё

#include 

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

Подробнее здесь: [url]https://stackoverflow.com/questions/79220242/find-the-position-of-an-object-if-it-is-specified-degrees-upwards-and-sideways-f[/url]