Помогите с проектом, соединение мотора с дальномером.

Есть цикл(выделен жирным шрифтом), который вращает дальномер на серве и определяет самое дальнее расстояние и угол под которым находится эта точка. Подскажите, как направить в эту точку робота, если мы имеем угол её нахождения.
#include <Servo.h>
const int motor1pin1 = 6;
const int motor1pin2 = 7;
const int motor2pin1 = 46;
const int motor2pin2 = 45;
const int Trig = 2; // обозначим к какой ножке и что подключаем
const int Echo = 3;
int i;
unsigned int time_us = 0; // Переменная для хранения временного интервала
unsigned int distance_sm = 0; // Переменная для хранения расстояния в сантиметрах
int chs;
Servo myServo;
int c = 0;
int y = 0;
void setup()
{
pinMode(motor1pin1, OUTPUT);
pinMode(motor1pin2, OUTPUT);
pinMode(motor2pin1, OUTPUT);
pinMode(motor2pin2, OUTPUT);
myServo.attach(9);
}

void loop() {
unsigned int j;

digitalWrite(Trig, HIGH); // Подаем сигнал на выход микроконтроллера
delayMicroseconds(10); // Удерживаем 10 микросекунд
digitalWrite(Trig, LOW); // Затем убираем
delay(500);
if (distance_sm > 10 ) forward();//пока расстояние больше 10 едем вперед
else {
reverse();//едем назад
delay(1000);//3 секунды
for (j = 0; j < 180; j + 20)
{
myServo.write(j);
digitalWrite(Trig, HIGH); // Подаем сигнал на выход микроконтроллера
delayMicroseconds(10); // Удерживаем 10 микросекунд
digitalWrite(Trig, LOW); // Затем убираем
delay(500);
if (distance_sm > c)
{
c = distance_sm;
y = j;
}
Serial.println(c);
Serial.println(y);
}
}
}
void forward() {
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, HIGH);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
}
void right() {
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
}
void left() {
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, HIGH);
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
}
void reverse() {
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
}
void stopm() {
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, LOW);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, LOW);
}

 

не в даваясь в дебри что там у вас меряется -
серва поворачивается с дальномером на какой то угол, например 5 гр - делаем замер
опять 5 гр и тд
после прохода 180 гр можем определить в каком положении макс значение и направление поворота робота.
поворачиваем платформу до тех пор, пока макс значение не будет при положении сервы 90 гр

 

Ну да Если не влезать в дебри инверсной кинематики )) (погуглите если интересно и с математикой в школе дружили) это самый простой способ. Угол у Вас остается в y. Если меньше у<90 давите влево, если больше доварачивайте вправо, продолжая мерить пока угол 90 не станет