Решение задач № 17 и 20

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

В первой задаче мы вернемся к истокам робототехнических Лего-состязаний и вспомним кегельринг. Давайте "вывернем его идею наизнанку" и теперь наш робот должен будет затолкать банки в центр круга.

Программа для решения задачи на языке NXC:

// Создаем переменные
float x,d;
int speed,es,D1,D2,e,u,PD,Pk;

task main()
{
  // Инициализируем датчики
  SetSensorLight(IN_2); // Датчик освещенности слева 2й порт
  SetSensorLight(IN_3); // Датчик освещенности справа 3й порт
  SetSensorLowspeed(IN_4); // Ультразвуковой датчик - 4й порт

  x = 180; // Ширина колеи
  d = 43.2; // Диаметр колес робота
  speed = 70; // Средняя скорость 
  es = 0; // "старая" ошибка ПД - регулятора
  Pk = 2; // Пропорциональный коэф-т ПД-регулятора
  PD = 4; // Дифференциальный коэф-т ПД-регулятора
  
  OnFwdSync(OUT_BC, 20, -100); // разворачиваемся в поисках банки
  while(SensorUS(IN_4) >= 50); // ожидаем банку
  while (SensorUS(IN_4) <= 50); // ожидаем потерю банки
  Off(OUT_BC); // останавливаем моторы
  // разворачиваемся для выезда рядом с банкой
  RotateMotorEx(OUT_BC, speed, x/d*85, -  100, true, true);
  Wait(50);
  OnFwdSyncEx(OUT_BC, 50, 0,RESET_NONE);
  // едем вперед пока не обнаружена линия
  while(Sensor(IN_3) >= 44 && Sensor(IN_2) >= 40.5);
  Off(OUT_BC); // останавливаемся
  Wait(50);
  // немного вперед, чтобы развернуться над линией
  RotateMotorEx( OUT_BC , speed , 200 , 0 , true , true );
  // поворачиваем вправо, УЗ-датчик смотрит в круг
  RotateMotorEx(OUT_BC , speed , x / d * 75, -100, true , true );
  // цикл для 4 банок
  for(int i=0; i<4; i++)
  {
    // пока нет банки - едем по линии на ПД-регуляторе
    while(SensorUS(IN_4) >=10)
    {
      // показания с датчиков
      D1 = Sensor(IN_3);
      D2 = Sensor(IN_2);
      // ошибка регулятора - разница показаний
      e = D2-D1;
      // управляющее воздействие по формуле ПД-регулятора
      u = e*Pk+PD*(e-es);
      // рассчитанную мощность подаем на моторы
      OnFwd(OUT_B,u+speed);
      OnFwd(OUT_C,-u+speed);
      // сохраняем ошибку для следующей итерации цикла
      es = e;
    }
    // останавливаемся при обнаружении банки
    Off(OUT_BC);
    // поворачиваемся к банке
    RotateMotorEx(OUT_BC , speed , x / d * 95, -100, true , true );
    Wait(50);
    // заталкиваем банку в центр круга
    RotateMotorEx( OUT_BC , speed , 1100 , 0 , true , true );
    Wait(50);
    // возвращаемся на линию
    RotateMotorEx( OUT_BC , -1*speed , 1100 , 0 , true , true );
    // разворачиваемся влево, чтобы продолжить движение
    RotateMotorEx(OUT_BC , speed , x / d * 87, 100, true , true );
  }
}

// создаем много переменных
int B_enkoder,C_enkoder,D1,D2,trigger;
bool out;
int e,u,Pk,PD,speed,es;
float d,x;

task main()
{
  // инициализируем датчики освещенности
  SetSensorLight(IN_2); 
  SetSensorLight(IN_3);

  x = 180; // ширина колеи
  d = 43.2; // Диаметр колес робота
  Pk = 2; // Пропорциональный коэф-т ПД-регулятора
  PD = 6; // Дифференциальный коэф-т ПД-регулятора
  speed = 75; // Средняя скорость 

  while(true)
  {
    // сбрасываем показания энкодеров
    ResetRotationCount(OUT_B);
    ResetRotationCount(OUT_C);
    // сохраняем показания энкодеров
    B_enkoder = 0;
    C_enkoder = 0;    
    es = 0; // "старая" ошибка ПД - регулятора
    out = true; // по изменению этой переменной будем вылетать из цикла
    Wait(10);
    while(out)
    {
      // показания с датчиков
      D1 = Sensor(IN_3);
      D2 = Sensor(IN_2);
      // ошибка регулятора - разница показаний
      e = D2-D1;
      // управляющее воздействие по формуле ПД-регулятора
      u = e*Pk+PD*(e-es);
      // рассчитанную мощность подаем на моторы
      OnFwd(OUT_B,u+speed);
      OnFwd(OUT_C,-u+speed);
      // сохраняем ошибку для следующей итерации
      es = e;
      // если проехали половину круга - вылетаем из цикла
      if ((MotorRotationCount(OUT_B)+MotorRotationCount(OUT_C))/2*(d*PI/360) >= (1070*PI)/2)
      {
        out = false;
      }
    }
    // стоп моторы
    Off(OUT_BC);
    // это пауза 50 мс
    Wait(50);
    
    // смотрим на каком энкодере набралось больше показаний
    // в зависимости от этого разворачиваемся к соответствующую
    // сторону и сохраняем в trigger
    if (MotorRotationCount(OUT_B)-B_enkoder>MotorRotationCount(OUT_C)-C_enkoder)
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, -100, true, true);
      trigger = 1;
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, 100, true, true);
      trigger = 2;
    }
    // выравниваемся о линию     OnRev(OUT_B,15);
    OnRev(OUT_C,15);
    while(Sensor(IN_3) >= 46 && Sensor(IN_2) >= 44)
    {
      if (Sensor(IN_3)<=44)
      {
        Off(OUT_C);
      }
      if(Sensor(IN_2)<=40.5)
      {
        Off(OUT_B);
      }
    }
    Off(OUT_BC);
    Wait(50);
    // едем через круг
    OnFwdSyncEx(OUT_BC, speed, 0,RESET_NONE);
    while(Sensor(IN_3) >= 44 && Sensor(IN_2) >= 40.5)
    {
    }
    // при обнаружении линии - останавливаемся
    Off(OUT_BC);
    Wait(50);
    // немного вперед, чтобы развернуться над линией
    RotateMotorEx(OUT_BC, speed, 200, 0, true, true);
    // в trigger - направление разворота
    if (trigger == 1)
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, 100, true, true);
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, -100, true, true);
    }
    // сохраняем показания энкодеров
    B_enkoder = MotorRotationCount(OUT_B);
    C_enkoder = MotorRotationCount(OUT_C);
  }
}