Многопоточный робот

Я смотрю на это, чтобы аналогичный проект. Это кажется легким и достаточно простым, чтобы вы могли быстро опробовать его. Преобразование этих функций в потоки не должно занять слишком много времени.

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

Я думаю, что это требует серьезной доработки. Сначала здесь:

  for(int i=0;i<1023;i++)
  {
  analogWrite(which, i);  
  }

Максимальное число аналоговой записи — 255, поэтому измените 1023 на 255.

Далее избавьтесь от вызовов метода задержки().

Давайте посмотрим на turnLeft :

// Чтобы повернуть робота на левую сторону
void turnLeft()
{
  stopmotor(M2);
  stopmotor(M3);
  delay(5000);
  Runmotor(M2);
  Runmotor(M3); 
}

Зачем останавливать двигатель, а затем запускать его снова? Вы имеете в виду:

// Чтобы повернуть робота на левую сторону
void turnLeft()
{
  stopmotor(M1);
  stopmotor(M4);
  delay(5000);
  Runmotor(M2);
  Runmotor(M3); 
}

Я все равно не понимаю, к чему приводит задержка. Избавьтесь от этого:

// Чтобы повернуть робота на левую сторону
void turnLeft()
{
  stopmotor(M1);
  stopmotor(M4);
  Runmotor(M2);
  Runmotor(M3); 
}

На самом деле, я не понимаю, чего достигают какие-либо вызовы задержки(). Попробуйте удалить их.

См. мой пост Как делать несколько дел одновременно... например, готовить бекон и яйца, чтобы получить советы о том, как делать несколько дел одновременно. вещи без использования задержки().

Также рассмотрите возможность использования массивов. Вместо M1, M2, M3, M4 создайте массив:

const int NUMBER_OF_MOTORS = 4;
const int motorPins [NUMBER_OF_MOTORS] = { 3, 4, 6, 9 };

Теперь проще настроить их все для вывода:

// Инициализация двигателей

for (int i = 0; i < NUMBER_OF_MOTORS; i++)
  pinMode (motorPins [i], OUTPUT);

Также их проще запустить или остановить:

for (int i = 0; i < NUMBER_OF_MOTORS; i++)
  stopmotor (i);

Где сейчас находится стоп-мотор:

// Остановить двигатель
void stopmotor(int which)
{
  digitalWrite(motorPins [which],LOW);
}

(Отредактировано для добавления)

Я тоже не понимаю, чего это дает:

void Runmotor(int which)
{
  for(int i=0;i<1023;i++)
  {
 analogWrite(which, i);  
  }
}

Помимо того факта, что вы не сможете подняться до 1023, если вы остановитесь на 255, время, необходимое для выполнения всех этих аналоговых операций записи, составит чуть более 2 мс. Зачем беспокоиться? Почему бы просто не сделать это:

void Runmotor(int which)
{
  digitalWrite (which, HIGH);
}

Возможно, вам захочется взглянуть на LMX Дэвида Андерсона Легкий многозадачный руководитель.

У него есть очень хорошая подборка видеороликов о коде, который он сделал для Dallas Personal Robotics Group (DPRG).

Специальные видеоролики для LMX:

• Дэвид Андерсон о программном обеспечении для робототехники – часть 1
• Дэвид Андерсон о программном обеспечении для робототехники – часть 2

Вы можете использовать планировщик и просто запускать задачи с помощью:

#include <Scheduler.h>
...
void setup()
{
  ...
  Scheduler.start(servoSetup, servoLoop);
  Scheduler.start(sensorSetup, sensorLoop);
  ...
}

void servoSetup()
{
  S.attach(servo);
}

void servoLoop()
{
  int angle;
  for(angle = 0;angle <= 180; angle++) { 
    S.write(angle); 
    if (angle == 90) {
      delay(2000);
    }
  }
  delay(2000);
  ...
}
...

Задачи должны вызывать yield() или delay() для переключения контекста. Функция taskSetup вызывается задачей первым и один раз. Функция taskLoop неоднократно вызывается задачей. Это можно рассматривать как выполнение дополнительного скетча.

Подробнее см. в примерах скетчей.

Вы также можете попробовать мою библиотеку ThreadHandler

https://bitbu cket.org/adamb3_14/threadhandler/src/master/

Он использует планировщик прерываний, чтобы обеспечить переключение контекста без пересылки на метод yield() или задержки().

Я создал библиотеку, потому что мне нужно было три потока, и мне нужно, чтобы два из них выполнялись в определенное время, независимо от того, что делали остальные. Первый поток обрабатывал последовательную связь. Второй запускал фильтр Калмана с использованием умножения матрицы с плавающей запятой с библиотекой Eigen. И третьим был поток контура управления быстрым током, который должен был иметь возможность прерывать матричные вычисления.

Как это работает

Каждый циклический поток имеет приоритет и период. Если поток с более высоким приоритетом, чем текущий выполняющийся поток, достигает следующего времени выполнения, планировщик приостановит текущий поток и переключится на поток с более высоким приоритетом. Как только поток с высоким приоритетом завершает свое выполнение, планировщик переключается обратно на предыдущий поток.

Правила планирования

Схема планирования библиотеки ThreadHandler следующая:

1. Сначала высший приоритет.
2. Если приоритет одинаковый, то поток с самым ранним сроком выполнения выполняется первым.
3. Если у двух потоков одинаковый крайний срок, первым будет выполнен первый созданный поток.
4. Поток может быть прерван только потоками с более высоким приоритетом.
5. После выполнения потока он блокирует выполнение всех потоков с более низким приоритетом до тех пор, пока функция запуска не завершит работу.
6. Функция цикла имеет приоритет -128 по сравнению с потоками ThreadHandler.

Как использовать

Потоки можно создавать с помощью наследования C++

class MyThread : public Thread
{
public:
    MyThread() : Thread(priority, period, offset){}

    virtual ~MyThread(){}

    virtual void run()
    {
        //code to run
    }
};

MyThread* threadObj = new MyThread();

Или через createThread и лямбда-функцию

Thread* myThread = createThread(priority, period, offset,
    []()
    {
        //код для запуска
    });

Объекты потоков автоматически подключаются к ThreadHandler при их создании.

Чтобы начать выполнение созданных объектов потока, вызовите:

ThreadHandler::getInstance()->enableThreadExecution();