Arduino.ru

Есть цикл(выделен жирным шрифтом), который вращает дальномер на серве и определяет самое дальнее расстояние и угол под которым находится эта точка. Подскажите, как направить в эту точку робота, если мы имеем угол её нахождения.

#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);

}