roboforum.ru

/* Распознаем объект */
      ct_recognize();
      if (ct_object.found) {
/* Следим за объектом */
         /* Кинематика... */
         determine_object_coords(-(((float)(ct_object.centerX-2))-((float)(IMG_WIDTH))/2.0), -(((float)(ct_object.centerY))-((float)(IMG_HEIGHT))/2.0), ct_object.size);
         bool try_catch = object_in_capture_zone();
         update_servo_pos();
         determine_grip_position(distA, q1a, q2a, q3a, Xn, Yn, Zn);
         next_position(Xn, Yn, Zn, q1a, q2a, q3a, try_catch);

         uint8_t result = reverse_objective(distA, Xn, Yn, Zn, q1, q2, q3);
         switch (result) {
            case RO_OK:      // если объект достижим
               if (try_catch) {   // и если он находится в зоне захвата, то
                  if (grip_catch()) {   // дико кидаемся на него, цапаем, и если сцапали, то
                     reverse_objective(a3, X_BASKET, Y_BASKET, Z_BASKET+35.0, q1, q2, q3);   // вычисляем кинематику для движения на корзину
                     disable_movement_shaper();   // отключаем шейпер (ну а че медлить)
                     move_grip();   // молнией мчимся в корзину
                     init_ms_timer();   // заново инициализируем таймер, т.к. он юзался раньше шейпером
                     wait_ms(1000);      // путь до корзины большой, подождем :)
                     grip_release();      // отпускаем шарик... или палец бедолаги-оператора :D
                     return;         // ну вот и все, the end ;)
                  } else {      // ну а если не удалось сцапать, то
                     grip_release();      // разжимаем наши смертельные челюсти
                     disable_movement_shaper();   // отключаем шейпер
                     init_ms_timer();
                     wait_ms(400);      // ждем, пока массивные зубы вытащатся из окровавленной плоти пальца оператора
                     q2 = q2a;      // возвращаемся в засаду, откуда накинулись
                     q3 = q3a;
                     enable_movement_shaper();   // включаем шейпер, торопитсья некуда, продолжаем охоту....
                  }
               }
               break;
            case RO_TOOFAR:   // если объект далеко, то
               target_orientation(Xn, Yn, Zn, q1, q2, q3);   // тянемся, тянемся до него, авось удастся порвать цепи и вырваться %)
               break;
         }

         move_grip();   // че-то там выше навычисляли, двигаемся туды...
      }