суббота, 3 декабря 2016 г.

Решение задач №5 и 6

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



В первой задаче наш робот будет объезжать препятствия на линии и возвращаться на нее для продолжения движения. 
Решение задачи на языке NXC:

float x,d,speed,es,D1,D2,e,u,PD,Pk;

task main()
{
  // инициализация датчиков
  SetSensorLight(IN_2);
  SetSensorLight(IN_3);
  SetSensorLowspeed(IN_4);
  // размер колеи
  x = 180;
  // диаметр колес
  d = 43.2;
  // средняя скорость
  speed = 30;
  // "старая" ошибка Д-регулятора
  es = 0;
  // пропорциональный коэф-т
  Pk = 2;
  // дифференциальный коэф-т
  PD = 4;

  // сбрасываем энкодеры  
  ResetRotationCount(OUT_BС);

  while(true)
  {
    // движемся по линии пока не обнаружим препятствие
    while(SensorUS(IN_4) > 28)
    {
      // показания датчиков линии
      D1 = Sensor(IN_2);
      D2 = Sensor(IN_3);
      // ошибка регулятора
      e = D2-D1;
      // управляющее воздействие
      u = e*Pk+PD*(e-es);
      // подаем его на моторы
      OnFwd(OUT_B,u+speed);
      OnFwd(OUT_C,-u+speed);
      // сохраняем ошибка для Д-регулятора
      es = e;
    }
    // моторы - стоп
    Off(OUT_BC);
    // поворот в сторону внутреннего изгиба трассы
    if (MotorRotationCount(OUT_B)>MotorRotationCount(OUT_C))
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, -100, true, true);
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, 100, true, true);
    }
    Wait(500);
    // съезжаем с линии
    RotateMotorEx(OUT_BC, speed, 1000, 0, true, true);
    // поворот в сторону линии
    if (MotorRotationCount(OUT_B)<MotorRotationCount(OUT_C))
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, -100, true, true);
      OnRev(OUT_B,-18);
      OnRev(OUT_C,-15);
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, 100, true, true);
      OnRev(OUT_B,-15);
      OnRev(OUT_C,-18);
    }
    Wait(500);
    
    // едем до линии
    while(Sensor(IN_3) >= 46 && Sensor(IN_2) >= 44)
    {
      if (Sensor(IN_3)<=46)
      {
        Off(OUT_C);
      }
      if(Sensor(IN_2)<=44)
      {
        Off(OUT_B);
      }
    }
    Off(OUT_BC);
    Wait(1000);
    // пересекаем линию
    RotateMotorEx(OUT_BC, 25, 200, 0, true, true);
    Wait(1000);
    // разворот для продолжения движения по линии
    if (MotorRotationCount(OUT_B)>MotorRotationCount(OUT_C))
    {
      RotateMotorEx(OUT_BC, 25, x/d*90, -100, true, true);
    }
    else
    {
      RotateMotorEx(OUT_BC, 25, x/d*30, 100, true, true);
    }
  }
}

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

float speed,es,D1,D2,e,u,PD,Pk,speednew1,speednew2;
long time1,time2,time3;
float es1,PD1,PD2,D,es2,e2,e1,u1,u2,Pk1,Pk2;x,d;y;
bool t,r;
int robot;

task main()
{
  // инициализация дальномера
  SetSensorLowspeed(IN_4);
  // направление синхронного разворота
  r = true;
  // номер робота 1 или 2
  robot = 1;
  // у нас в роботах разные датчики, поэтому разная инициализация
  if(robot == 2)
  {
    SetSensorLight(IN_2);
    SetSensorLight(IN_3);
    // один робот будет убегать
    t = true;
  }
  else
  {
    SetSensorColorRed(IN_2);
    SetSensorColorRed(IN_3);
    // другой робот будет догонять
    t = false;
  }

  while(true)
  {
    // размер колеи
    x = 180;
    // диаметр колес, мм
    d = 43.2;

    if (t == true)
    {
      // таймер для синхронного разворота
      time2 = CurrentTick();
      // таймер для периодической смены скорости
      time1 = CurrentTick();
      // первые 2 секунды робот равняется, вперед не едет
      speed = 0;
      // "старая" ошибка Д-регулятора
      es = 0;
      // пропорциональный коэф-т
      Pk = 2;
      // дифференциальный коэф-т
      PD = 4;
      // таймер для 2-секундного выравнивания перед началом движения 
      time3 = CurrentTick();
      // запоминаем, было ли уже выравнивание
      y = 0;
      // фактическая средняя скорость      
      speednew1 = 0;

      while(true)
      {
        // через две секунды начинаем движение после выравнивания
        if(CurrentTick() > time3+2000 && y == 0)
        {
          speednew1 = speed + Random(80);
          y = 1;
        }

        // смена ролей между роботами каждые 29 секунд
        if(CurrentTick() > time2+29000)
        {
          break;
        }
        
        // каждые 5 секунд смена скорости
        if(CurrentTick() > time1+5000)
        {
          speednew1 = speed + Random(80);
          time1 = CurrentTick();
        }

        // показания датчиков
        D1 = Sensor(IN_3);
        D2 = Sensor(IN_2);
        // ошибка регулятора
        e = D2-D1;
        // управляющее воздействие ПД-регулятора
        u = e*Pk+PD*(e-es);
        // подаем его на моторы
        OnFwd(OUT_B,u+speednew1);
        OnFwd(OUT_C,-u+speednew1);
        // запоминаем ошибку для Д-регулятора
        es = e;
      }
      // моторы - стоп
      Off(OUT_BC);
      Wait(1000);
      // смена ролей - разворот
      if (r == true) RotateMotorEx(OUT_BC, 30, x/d*150, 100, true, true);
      else RotateMotorEx(OUT_BC, 30, x/d*150, -100, true, true);
      Wait(1000);
      t = !t;
      r = !r;
    }
    else
    {
      time2 = CurrentTick();
      time3 = CurrentTick();
      speed = 0;
      es1 = 0;
      PD1 = 0;
      Pk1 = 2.5;
      es2 = 0;
      Pk2 = 2;
      PD2 = 6;
      y = 0;

      while(true)
      {
        if(CurrentTick()> time3+2000 && y == 0)
        {
          speed = 50;
          y = 1;
        }
        if(CurrentTick() > time2+29000)
        {
          break;
        }

        D = SensorUS(IN_4);
        D2 = Sensor(IN_2);
        D1 = Sensor(IN_3);
        
        // ограничиваем зону видимости робота - 60 см
        if (D > 60)
        {
          D = 60;
        }

        // здесь у нас целых два регулятора
        // один ведет по линии
        // второй - подстраивает скорость движения
        e2 = D2 - D1;
        e1 = D - 40;
        u2 = e2*Pk2+PD2*(e2-es2);
        u1 = e1*Pk1+PD1*(e1-es1);
        OnFwd(OUT_B,speed+u1+u2);
        OnFwd(OUT_C,speed+u1-u2);
        
        es1 = e1;
        es2 = e2;
      }
      Off(OUT_BC);
      Wait(1000);
      // смена ролей - разворот
      if (r == true) RotateMotorEx(OUT_BC, 30, x/d*150, 100, true, true);
      else RotateMotorEx(OUT_BC, 30, x/d*150, -100, true, true);
      Wait(1000);
      t = !t;
      r = !r;
    }
  }
}

На видео можно посмотреть как роботы решают эти задачи:



Комментариев нет:

Отправить комментарий

Самое популярное