Конвертируем скорость автомобиля

В этом уроке:

• Для чего необходимо изменить задание скорости
• Создаём функцию, конвертирующую скорость

Видео версия урока

Проблема задания скорости

Мы уже почти готовы перейти к следующей главе и работать со знаками и светофорами. Но перед этим необходимо учесть еще один момент — как мы задаём скорость для моторов.

До этого момента мы задавали скорость движения машинки в процентах от максимальной и предполагали, что аналогия в нашей жизни — это скорость в километрах/час. Например, скорость 60% означала скорость 60 км/ч. Однако, бывают ситуации, когда нужно двигаться очень медленно или очень быстро. 

Например, задание скорости 10 км/ч, означает, что процент заполнения ШИМ будет равен 10, а при таком значении машинке может не хватить мощности тронуться с места. Или же напротив, ограничение скорости 110 км/ч (на автомагистралях) не позволит ехать со скоростью, выше физически возможной для моторов.

Чтобы избежать таких ситуаций, необходимо преобразовать скорость из км/ч в % ШИМ.

Пишем функцию для преобразования скорости

Создадим функцию (77 строка), которая будет принимать значения в км/ч и возвращать значения в % ШИМ.

float convertSpeed(float speedLevel){                   // Функция преобразования скорости из км/ч в % ШИМ
  if (speedLevel == 0) return 0;                        // Если скорость равна 0, возвращаем 0
  else if (speedLevel < 20) return 20;                  // Если скорость меньше минимальной, устанавливаем минимальное заполнение ШИМ
  else if (speedLevel > 90) return 60;                  // Если скорость больше максимальной, устанавливаем максимальное заполнение ШИМ
  else return map(speedLevel, 20, 90, 20, 60);          // Преобразование диапазон остальных скоростей
}

Преобразование производим с помощью функции map() (81 строка), которая преобразовывает переменную из одного диапазона значений в другой.

Синтаксис: map(value, fromLow, fromHigh, toLow, toHigh);

• value — переменная, которую нужно преобразовать (speedLevel);
• fromLow — нижняя граница прежнего диапазона (20);
• fromHigh — верхняя граница прежнего диапазона (90);
• toLow — нижняя граница нового диапазона (20);
• toHigh — верхняя граница нового диапазона (60).

Таким образом, переменная была в диапазоне значений от 20 до 90 км/ч, а станет — от 20 до 60 % ШИМ.

Также добавляем три условия:

1. Если машинка стоит (скорость равна 0), то и вернём 0 (78 строка);
2. Если скорость — от 1 до 20 км/ч, вернём 20% для того, чтобы четко ограничить минимальную скорость движения (79 строка). Это позволит однозначно указать, едет машинка или нет;
3. Если скорость — больше 90 км/ч, ограничим её максимально возможной: 60% ШИМ (80 строка).

Как выбрать диапазоны значений

Почему мы выбрали именно такие диапазоны значений? 

20 и 90 — минимальная и максимальная скорости движения автомобиля в км/ч, с которыми мы будем работать. Это те значения, которые мы будем указывать в скетче (полученные от знаков на дороге).

20 — минимальный процент ШИМ, при котором машинка уверенно едет с минимальной скоростью. Эта величина также зависит от настройки П-регулятора.

60 — максимальная скорость, при которой после вычисления по формуле П-регулятора, получается значение не больше 100. Мы помним, что не можем задать двигателю скорость вращения больше 100% ШИМ. Скорость всё равно будет округлена до 100.

Если мы поставим число больше 60%, то при сильном отклонении линии от центра коэффициент П-регулятора станет настолько большим, что скорость мотора увеличится недостаточно, и поворот будет не таким резким, каким должен быть. 

Скетч езды по линии с указанием скоростей в км/ч

#include <Wire.h>                         // Подключаем библиотеку для работы с аппаратной шиной I2C
#include <iarduino_I2C_Motor.h>           // Подключаем библиотеку для работы с мотором  I2C-flash
#include <iarduino_I2C_Bumper.h>          // Подключаем библиотеку для работы с бампером I2C-flash
                                          
iarduino_I2C_Motor mot_R (0x0A);          // Объявляем объект mot_R для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Motor mot_L (0x0B);          // Объявляем объект mot_L для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Bumper bum (0x0C);           // Объявляем объект bum  для работы с бампером I2C-flash, указав адрес модуля на шине I2C
                                          
const float min_Speed = 20;               // Минимальная  скорость движения в км/ч. Используется при движении по дорогам
const float mid_Speed = 60;               // Средняя      скорость движения в км/ч. Используется при движении по дорогам
float speed = min_Speed;                  // Указываем, что изначально скорость равна минимальной
int8_t  val_Turn      = 0;                // Выбранное направление движения на перекрёстке: 0 прямо, -1 влево, +1 вправо
bool    flg_CrossWait = true ;            // Флаг ожидания    перекрёстка (машина "увидела" светофор или знак, сообщающий о наличии перекрёстка)
bool    flg_CrossFind = false;            // Флаг обнаружения перекрёстка (бампер заехал на перекрёсток)
bool    flg_Turn = false;                 // Флаг необходимости разворота                  
     
void setup() {
  mot_R.begin();                          // Инициируем работу с левым  мотором I2C-flash
  mot_L.begin();                          // Инициируем работу с правым мотором I2C-flash
  bum.begin();                            // Инициируем работу с бампером I2C-flash
  mot_R.setDirection(true);               // Указываем правому мотору, что его вращение должно быть прямым (по часовой стрелке при положительных скоростях)
  mot_L.setDirection(false);              // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях)
}

void loop() { 
//    РАЗВОРОТ:
if (bum.getCross(3, 1000)) flg_Turn = true;// Если обнаружен перекрёсток, устанавливаем флаг разворота
if( flg_Turn ){                            // 1 ФАЗА. Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
                                           //         Под бампером обнаружен перекрёсток:
  flg_Turn = false;                        //         Сбрасываем флаг разворота
  mot_R.setSpeed( convertSpeed(min_Speed), MOT_PWM );    // Устанавливаем скорость правого мотора в %
  mot_L.setSpeed(-convertSpeed(min_Speed), MOT_PWM );    // Устанавливаем скорость левого мотора в %
                                           //         Ждём завершение манёвра:
  while( bum.getLineSum() > 0 ) {;}        // 2 ФАЗА. Ждём выхода линии за пределы бампера. Цикл выполняется, пока под бампером есть линия
  while( bum.getLineSum() == 0 ){;}        // 3 ФАЗА. Ждём появления линии под бампером.    Цикл выполняется, пока под бампером нет  линии
                                           //         Останавливаемся:
  mot_R.setStop();                         //         Останавливаем правое колесо
  mot_L.setStop();                         //         Останавливаем левое колесо
  val_Turn  = 0;                           // 4 ФАЗА. Выбираем движение прямо
  speed = mid_Speed;                       //         Снимаем наложенные ранее ограничения скорости
}

//   ПРОВЕРКА НАЛИЧИЯ ПЕРЕКРЁСТКОВ:                                       
  if( bum.getCross(3,1000) ){             // Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...                            
    if( flg_CrossWait ){                  // Если ожидается появление перекрёстка ...
      flg_CrossWait = false;              // Сбрасываем    флаг ожидания    перекрёстка
      flg_CrossFind = true;               // Устанавливаем флаг обнаружения перекрёстка
    }                                                               
  }
else{                                     // Если под бампером обычная линия ...          
     if (flg_CrossFind) {                 // Если ранее был обнаружен перекрёсток 
       flg_CrossFind = false;             // Сбрасываем флаг обнаружения перекрёстка
       val_Turn = 0;                      // Выбираем движение прямо
       speed = mid_Speed;                 // Снимаем наложенные ранее ограничения скорости
     }                                                               
  }

if( val_Turn == -1 ){ bum.setTurnSignal(BUM_TURN_LEFT ); }  //  Если поворот будет осуществляться налево, включаем левый поворотник
if( val_Turn == 0 ) { bum.setTurnSignal(BUM_TURN_OFF  ); }  //  Выключаем поворотники, если движение прямо
if( val_Turn == 1 ) { bum.setTurnSignal(BUM_TURN_RIGHT); }  //  Если поворот будет осуществляться направо, включаем правый поворотник

//   ОПРЕДЕЛЯЕМ ОШИБКУ ЦЕНТРИРОВАНИЯ ЛИНИИ ПОД БАМПЕРОМ:                   
//   Если ожидается перекрёсток: машина должна ехать по центру линии, немного сместившись в сторону поворота
     float bum_Error;                                                      // Объявляем переменную: ошибка для П-регулятора
     if( flg_CrossWait ){ bum_Error = bum.getErrPID()+val_Turn;   }else    // Получаем ошибку центрирования линии, смещённую на 1 датчик в сторону ожидаемого поворота «val_Turn»
//   Если бампер заехал на перекрёсток:                                    // Машина должна ехать по краю линии, выполняя поворот
     if( flg_CrossFind ){ bum_Error = bum.getSidePID(val_Turn)*3; }else    // Получаем ошибку нахождения на краю линии, со стороны поворота «val_Turn». Умножаем ошибку — это увеличит резкость поворота (актуально для тонких линий трассы)
//   Если не ждём перекрёсток и не находимся на нём:                       // Машина должна ехать по центру линии
                        { bum_Error = bum.getErrPID();            }        // Получаем ошибку центрирования линии
                        
  float kP = 3 + 0.125*(convertSpeed(speed)-20);        // Коэффициент П-регулятора
  float P = bum_Error * kP;                             // Получаем значение от П-регулятора
  mot_R.setSpeed(convertSpeed(speed) - P, MOT_PWM);     // Устанавливаем скорость правого мотора 
  mot_L.setSpeed(convertSpeed(speed) + P, MOT_PWM);     // Устанавливаем скорость левого мотора 
}

float convertSpeed(float speedLevel){                   // Функция преобразования скорости из км/ч в % ШИМ
  if (speedLevel == 0) return 0;                        // Если скорость равна 0, возвращаем 0
  else if (speedLevel < 20) return 20;                  // Если скорость меньше минимальной, устанавливаем минимальное заполнение ШИМ
  else if (speedLevel > 90) return 60;                  // Если скорость больше максимальной, устанавливаем максимальное заполнение ШИМ
  else return map(speedLevel, 20, 90, 20, 60);          // Преобразование диапазона остальных скоростей
}