Остановите автомобиль робота, когда он обнаружит объект с помощью ультразвукового датчика (C ++) - PullRequest
Новая
       13

Остановите автомобиль робота, когда он обнаружит объект с помощью ультразвукового датчика (C ++)

1 голос
/ 02 ноября 2019

Недавно я пытался остановить своего робота, когда он обнаружит препятствие перед ним. Ситуация следующая: когда я обнаруживаю объект, машина останавливается, и это нормально, но когда объект исчезает, автомобиль не продолжает двигаться к своему конечному пункту назначения. Вместо этого автомобиль должен ехать из пункта А в пункт Б в течение 10 секунд (10 секунд - это просто пример, время отличается от каждого движения), и когда он обнаружит объект, он должен остановиться и подождать, пока объект не исчезнет. после этого он должен продолжить свой конечный пункт назначения (точка B).

У меня есть идея подсчитать время, пока робот не двигается, и добавить это время ко времени, необходимому для достижения конечной точки. Но я борюсь с этим.

Вот код: 

int measureDistance()
{
    if (wiringPiSetup() == -1)
        cout << "Initialization problem - measureDistance() " << endl;

    Sonar sonar;
    sonar.init(trigger, echo);

    int distance = 0;

    distance = sonar.distance(30000);
    sleep_for(nanoseconds(10));

    return distance;
}

bool checkForObstacles() 
{
    wiringPiSetup();

    // Controlling the motors from here
    softPwmCreate(0, 0, 255);
    softPwmCreate(4, 0, 255);

    constexpr int MIN_DISTANCE = 20;

    int distance = measureDistance();
    cout << "Distance: " << distance << endl;

    if (distance >= MIN_DISTANCE)
        return false;

    softPwmWrite(0, LOW);
    softPwmWrite(4, LOW);

    while(distance < MIN_DISTANCE) 
    {
        delay(10); // re-measure after 10ms. Adjust to what you prefer
        distance = measureDistance();
        cout << "Measuring: " << distance << "cm" << endl;
    }


    return false;
}

void move(int t)
{
        // Pins where the motors are connected
    int ena = 0;
    int in1 = 2;
    int in2 = 3;

    int enb = 4;
    int in3 = 5;
    int in4 = 6;
    // Pins setup
    wiringPiSetup ();

    softPwmCreate(ena, 0, 255);
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);

    softPwmCreate(enb, 0, 255);
    pinMode(in3, OUTPUT);
    pinMode(in4, OUTPUT);

    // Control
    softPwmWrite(ena, 50);
    digitalWrite(in1, 1);
    digitalWrite(in2, 0);

    softPwmWrite(enb, 50);
    digitalWrite(in3, 1);
    digitalWrite(in4, 0);

    cout << "TIME: " << t << endl;
    auto start = chrono::high_resolution_clock::now();
    while(true) 
    {
        auto now = chrono::high_resolution_clock::now();
        auto elapsed = chrono::duration_cast<chrono::milliseconds>(now-start).count();

        cout << "ELAPSED: " << elapsed << endl;

        int remaining = t - (int) elapsed;

        cout << "REMAINING: " << remaining << "ms" << endl;
        if (remaining < 0)
            break;

        if (checkForObstacles())
        {
            continue;
        }   

        delay(min(remaining, 25)); // replace 25 with how often you want to check the distance
    }

    softPwmWrite(ena, LOW);
    softPwmWrite(enb, LOW);
    delay(200);
}

Для возобновления: Если автомобиль движется, если я ставлю препятствие перед ним, он останавливается, но когда я удаляю препятствие, программа заканчивается- после этого машина не двигается. Этого не должно быть.

PS: Все работает на Raspberry Pi.

1 Ответ

0 голосов
/ 02 ноября 2019

Это своего рода псевдокод, потому что я не знаю, как точно работают ваш робот и wiringPi, но я надеюсь, что этот поток кода поможет вам в правильном направлении: measureDistance показалась довольно бесполезной, поэтому я удалил ее (и она называлась инициализациейкаждый раз по какой-то причине). В своей основной части вы должны настроить все булавочные вещи и сонар, затем в бесконечном цикле вы немного спите, проверяете препятствия, если их нет, вы двигаетесь, иначе пропускаете движущуюся часть. 

int main()
{
    // only initialize everything once, in your main
    if (wiringPiSetup() == -1)
        return -1;

    Sonar sonar;
    sonar.init(trigger, echo); // declare these somewhere first, I don't know the values

    while(1){
        sleep_for(nanoseconds(10)); // or delay(200) or whatever
        // if there are obstacles skip the move() part and go back to sleeping
        // or delay again, and try again
        if (checkForObstacles(sonar))
            continue;
        // No obstacle = move
        move();
    }
}

// this is only responsible for checking obstacles, no pin writing and moving happening
bool checkForObstacles(Sonar sonar) // pass in a reference to sonar
{
    constexpr int MIN_DISTANCE = 20;

    int distance = sonar.distance(30000);
    cout << "Distance: " << distance << endl;

    if (distance >= MIN_DISTANCE)
        return false;

    return true;
}

void move() {
    /*
    *  only move logic goes here
    * like the digitalWrites
    */
}
...