Skip to content

Глава 4. Робот с Delta-кинематикой ​

4.1. Обзор Delta-робота ​

Робот с Delta-кинематикой – вид параллельного робота, который состоит из трёх рычагов, прикреплённых посредством карданных шарниров к основанию. Ключевой особенностью является использование параллелограммов в конструкции манипулятора, что позволяет сохранять пространственную ориентацию исполнительного устройства робота.

Роботы с Delta-кинематикой (Delta-роботы) особенно популярны на линиях упаковки, поскольку они достаточно быстры, некоторые осуществляют до 300 захватов в минуту. Delta-робота придумал в начале 1980-х годов швейцарский учёный Реймонд Клавель. Ниже приведена иллюстрация из оригинального патента US4976582 на «Устройство для перемещения и позиционирования элемента в пространстве» (Рис. 4.1).

Робот состоит из двух платформ: неподвижно закреплённого верхнего основания (1) и небольшой подвижной платформы (8), соединённых тремя рычагами. Каждый рычаг состоит из двух частей: верхнее плечо (4) жёстко соединено с двигателем (3), расположенным на верхнем основании, а нижнее представляет собой параллелограмм (5), в углах которого установлены так называемые универсальные шарниры (6, 7), которые позволяют углам изменяться. Каждый параллелограмм соединён с верхним рычагом шарниром (16) таким образом, чтобы его верхняя сторона всегда оставалась перпендикулярной своему рычагу и параллельной плоскости верхнего основания. Благодаря этому, подвижная платформа робота, прикреплённая к нижним сторонам параллелограммов, также будет всегда параллельной верхнему основанию. Управлять положением платформы становится возможным, изменяя угол поворота верхних рычагов относительно основания робота при помощи двигателей. В центре нижней платформы (8) крепится рабочий орган робота (9). Это может быть манипулятор, захватывающее устройство или, например, экструдер (в случае 3D принтера). Дополнительно может использоваться ещё один двигатель (11), который обеспечивает вращение рабочего органа через штангу (14). Тяжёлые двигатели размещены на неподвижном основании, движутся только рычаги и нижняя платформа, которые стараются изготавливать из лёгких композитных материалов, тем самым уменьшая их инерцию.

image194

Схемотехническое изображение Delta-робота

4.2. Обратная задача кинематики Delta-робота ​

Для корректного управления Delta-роботом, необходимо научиться решать обратную (инверсную) кинематическую задачу. Исходя из известной позиции, в которую требуется переместить манипулятор робота (например, схватить объект, который находится на конвейере в точке с координатами x, y, z), требуется определить величины углов, на которые необходимо повернуть двигатели, связанные с рычагами робота, чтобы установить его в правильное положение для захвата.

Формализованная задача выглядит следующим образом: неподвижное основание робота и его движущуюся платформу можно представить в виде равносторонних треугольников. На представленной ниже схеме они закрашены зелёным и розовым цветами, соответственно. Углы поворота рычагов робота относительно плоскости основания (они же — углы поворота моторов) обозначены как

а координаты точки E0, расположенной в центре подвижной платформы и в которой в реальной жизни будет закреплён манипулятор робота — как .Таким образом, необходимо придумать функцию для решения обратной кинематической задачи:

Зададим несколько ключевых параметров, которые определяются геометрическими размерами нашего робота (Рис. 4.2).

Обозначим длину стороны верхнего основания f, сторону нижней платформы e, длину верхнего плеча рычага rf и длину нижнего плеча (длинной стороны параллелограмма) re. Для вычислений выберем систему координат с точкой отсчёта, совпадающей с геометрическим центром верхнего треугольника. Ось Z направим вверх, таким образом, z-координата подвижной платформы всегда будет отрицательной.

Конструкция робота подразумевает, что рычаг F1J1 (Рис. 4.3) может вращаться лишь в плоскости YZ, описывая при этом окружность радиусом rf с центром в точке F1 (именно в этом месте он прикреплён к двигателю). В отличие от F1, в узлах J1 и E1 используются универсальные шарниры, благодаря которым плечо E1J1 может свободно вращаться относительно J1, описывая сферу радиусом re с центром в точке E1.

Пересечением этой сферы и плоскости YZ является окружность с центром в точке E’1 радиусом E’1J1, где точка E’1 находится как проекция точки E1 на плоскость YZ. Тогда точка J1 будет находиться на пересечении двух окружностей с центрами в точках E’1 и F1, причём радиусы этих окружностей мы можем определить. Здесь присутствует небольшая тонкость: окружности пересекаются в двух точках, но нас интересует только одна из них — с меньшим значением координаты y, поскольку мы хотим, чтобы рычаги робота всегда торчали «локтями» наружу. Определив таким образом координаты точки J1, мы легко сможем найти интересующий нас угол

Для удобства восприятия ниже показана проекция нашей трёхмерной картинки на плоскость YZ (Рис. 4.4).

Нижняя платформа - равносторонний треугольник, центром которого является точка .

Значит, расстояние E0E1 можно рассчитать по следующей формуле:

Eqn18

Eqn19

Eqn16

image195

image196

Схематические изображения Delta-робота для решения обратной кинематической задачи

image197

Схематические изображения Delta-робота для решения обратной кинематической задачи

image198

Eqn20

Проекция трехмерного схематического изображения Delta-робота на плоскость YZ

Eqn21

Eqn22

что даёт нам следующие координаты точки E1 и её проекции E’1 на плоскость YZ:

Расстояние E1E’1=x0, тогда, согласно теореме Пифагора:

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

Чтобы найти координаты точки J1, являющейся пересечением двух окружностей, надо решить систему уравнений:

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

После раскрытия скобок и вычитания одного уравнения из другого, можно линейным образом выразить z-координату точки J1 через y-координату:

Подставив определенную выше z-координату во второе уравнение, получим обычное квадратное уравнение относительно y. Из двух решений уравнения выберем наименьшее (как это обсуждалось выше). Получив таким образом координаты точки J1, найдём и угол:

Eqn28

Eqn23

Eqn27

Eqn26

Eqn24

Eqn29

Eqn25

Все выражения получились достаточно простыми благодаря удачному выбору системы координат: плечо рычага F1J1 всегда движется в плоскости YZ, поэтому координату x можно просто не учитывать. Чтобы сохранить это преимущество при нахождении двух оставшихся углов Θ2 и Θ3, воспользуемся симметрией конструкции Delta-робота. Сначала повернём нашу систему координат на 120° против часовой стрелки в плоскости XY вокруг оси Z (Рис.4.5).

В результате получается новая система координат X’Y’Z’, и в этой новой системе можно воспользоваться готовыми формулами для нахождения угла Θ2. Единственная тонкость заключается в том, что необходимо предварительно пересчитать координаты точки E0 в новой системе отсчёта. Это легко сделать при помощи известной формулы (преобразования при повороте системы вокруг начала координат), показанной на рисунке выше. Для нахождения угла Θ3 необходимо будет также повернуть исходную систему отсчёта, но теперь уже по часовой стрелке. Этот приём очень удобно реализуется в виде программы: достаточно написать функцию для вычисления угла Θ в плоскости YZ, а затем вызвать её три раза для каждого из углов и систем отсчёта.

image206

Поворот системы координат на 120 градусов вокруг оси Z в плоскости XY

image207

Инструкция по сборке робота, а также исходные файлы конструктивных элементов под лазерную резку находятся в свободном доступе и доступны для скачивания с официального сайта appliedrobotics.ru из раздела «Учебные материалы». На верхней неподвижной платформе установлено три сервопривода DYNAMIXEL AX-12A (Рис. 4.6). В качестве управляющего контроллера используется Arduino-подобная аппаратная платформа OpenCM, установленная на плате расширения 485 Expansion Board (также возможно использование платы STEM Board). Питание обеспечивается при помощи либо стандартного LiPo аккумулятора, выдающего номинальное напряжение в 11,1 В, либо через сетевой адаптер питания. Соединение сервоприводов рекомендуется выполнить последовательно и подключить получившуюся цепь к плате расширения 485 Expansion Board как на рисунке 4.7. Перед тем как приступить к разработке управляющей программы, необходимо уточнить ID сервоприводов, установленных на манипуляторе. ID сервоприводов указаны на рисунке 4.7.

Для уточнения и установки ID сервоприводов можно использовать стандартные примеры из Arduino IDE с установленной поддержкой платы OpenCM9.04. Для этого необходимо открыть два примера под названием: “o_Find_Dynamixel” и “c_ID_Change”. (Файл => Примеры => OpenCM9.04 => DynamixelWorkbench).

С помощью примера “o_Find_Dynamixel” можно узнать текущее ID сервопривода следующим образом: сервопривод подключается к плате, подключается питание, и загружается пример на плату. После чего в Мониторе порта можно увидеть информацию о текущем значении ID, которое необходимо запомнить.

После этого откройте второй пример - “c_ID_Change”. В оглавлении, при необходимости, нужно изменить эти строки:

#define DXL_ID 1 // 1 – это текущее значение ID сервопривода

#define NEW_DXL_ID 2 // 2 – это значение, на которое требуется поменять ID

Помимо этого, в обоих случаях необходимо поменять скорость “общения” платы со входами Dynamixel

#define BAUDRATE 57600 // изменяем значение на 1 000 000

Таким образом, устанавливаются необходимые ID для сервоприводов.

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

4.3. Устройство Delta-робота ​

Собранный из оргстеклянных пластин Delta-робот

5

Схема подключения и назначения ID сервоприводов

4.4. Разработка управляющей программы ​

В модели данного робота в качестве управляющего контроллера используется Arduino-подобный контроллер OpenCM. Для работы с ним используется среда разработки Arduino IDE.

Для работы с сервоприводами DYNAMIXEL нужно подключить библиотеку и инициализировать переменные:

cpp
#include <DynamixelWorkbench.h>
DynamixelWorkbench dxl_wb;

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

#define Device_Name “1” – подключение к порту на плате OpenCM9.04

#define Device_Name “2” – подключение через USB переходник или через Bluetooth

#define Device_Name “3” – подключение к плате расширения OpenCM 485 Expansion Board или STEM Board

В рассматриваемом примере требуется выбрать 3 вариант

Указывается скорость обмена данными через порт подключения:

cpp
#define BAUDRATE  1000000

После чего нужно указать ID сервоприводов, которыми необходимо управлять. Все это делается в коде программы до функции setup() при помощи следующих команд:

cpp
#define DXL_ID1    1
#define DXL_ID2    2
#define DXL_ID3    3 // Объявление ID сервоприводов манипулятора (1-3)

Также инициализируются геометрические данные Delta-робота в качестве глобальных переменных. Они необходимы для расчетов. В рассматриваемой модели робота нужно инициализировать следующие переменные:

cpp
int f = 45;
int Rf = 100;
int Re = 210;
int e = 20;
float theta1, theta2, theta3 ; // Угол поворота вала двигателя
float y1, y2, y3; // Координаты каждой точки на верхней платформе
float x1, x2, x3;
float m, n, k;

Функция setup()

Создаем переменные «model_number» и присваиваем им номер сервопривода. После производим подключение всех сервоприводов манипулятора.

Записываем функцию инициализации:

cpp
dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
Serial.begin(1000000);
const char *log ;
uint16_t model_number1 = 1;
uint16_t model_number2 = 2;
uint16_t model_number3 = 3;
dxl_wb.ping(DXL_ID1, &model_number1, &log);
dxl_wb.ping(DXL_ID2, &model_number2, &log);
dxl_wb.ping(DXL_ID3, &model_number3, &log);

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

cpp
dxl_wb.jointMode(DXL_ID1, 50, 0, &log);
dxl_wb.jointMode(DXL_ID2, 50, 0, &log);
dxl_wb.jointMode(DXL_ID3, 50, 0, &log);

Где значение 50 является скоростью, а 0 – ускорение.

Теперь рассмотрим действие каждой функции отдельно.

Функция coord()

В этой функции осуществляется пересчёт координат точки Е0 в других системах отсчёта.

cpp
void coord(float a, float b)
{
  x1 = a; // координаты первой точки
  y1 = b;
  x2 = a*cos(2.094) + b*sin(2.094); // координаты второй точки
  y2 = b*cos(2.094) - a*sin(2.094);
  x3 = a*cos(2.094) - b*sin(2.094); // координаты третьей точки
  y3 = b*cos(2.094) + a*sin(2.094);
}

Функция find_angle()

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

cpp
void find_angle(float x, float y, float z)
{
   float C1 = -0.5 * 0.57735 * f; // f/2 * tg 30
   y -= 0.5 * 0.57735 * e;
   // z = a+b*yj
   float  a = (x*x + y*y + z*z +Rf*Rf - Re*Re - C1*C1)/(2*z);
   float b = (C1-y)/z;
   float d = -(a+b*C1)*(a+b*C1)+Rf*(b*b*Rf+Rf);
   if(d < 0)  // Осуществление проверки дискриминанта: если он меньше нуля, углы обнуляются, и сервоприводы возвращаются в базовое положение при 0°
{
   Serial.println(«Error: Discriminant!»);
      theta1 = 0;
      theta2 = 0;
      theta3 = 0;
   }
  else
{
  float yj = (C1 - a*b - sqrt(d))/(b*b + 1); // выбираем внешнюю точку
  float zj = a + b*yj;
 float angle = atan(-zj/(C1 - yj));
  theta = angle;
    }
 }

Функция full()

В этой функции осуществляется нахождение трех углов для каждого двигателя.

cpp
void full(float x0, float y0, float z0)
{
// Рассчитаем в какую точку переместится манипулятор при введенных координатах.
// Радиус действия робота составляет 120 мм. Если заданная точка больше радиуса,
// в консоли появится ошибка и манипулятор вернется в базовое положение при 0°
  float R1 = sqrt(x0*x0+y0*y0);
// Также проверим на какую высоту поднимется/опустится нижняя платформа
  if( R1>120 || z0 < -200 || z0 > -150)
  {
      Serial.println(«Error: Radius!»);
      theta1 = 0;
      theta2 = 0;
      theta3 = 0;
  }
else
{
    coord(x0, y0); // Расчёт координат каждой точки
    find_angle(x1, y1, z0); // Расчёт угла для первого двигателя
    theta1 = theta;
    find_angle(x2, y2, z0); // Расчёт угла для второго двигателя
    theta2 = theta;
    find_angle(x3, y3, z0); // Расчёт угла для третьего двигателя
    theta3 = theta;
}

Функция set_pos()

В данной функции осуществляется задание угла поворота вала двигателя на манипуляторе

cpp
void set_pos( float p1, float p2,  float p3, int del)  // Поворот задается в радианах, del – задержка
{ dxl_wb.goalPosition(DXL_ID1,p1);
  dxl_wb.goalPosition(DXL_ID2,p2);
  dxl_wb.goalPosition(DXL_ID3,p3);
  delay(del);
}

Теперь вызовем функцию loop(). Данная функция представляет собой бесконечный цикл. Например, зададим нижней платформе манипулятора перемещение в координаты х = 0; у = 0; z = -200;

cpp
void loop()
{
 full(0,0,-200);
set_pos(theta1, theta2, theta3, 1000); // Поворот валов сервоприводов на вычисленный угол – достижение заданных координат
}
// Также можно корректировать положения робота в реальном времени. С помощью консоли пользователь может вводить координаты, и робот будет их занимать.
// Редактируем функцию loop():
if (Serial.available() > 0 ) // Функция, проверяющая наличие символов, введенных в консоль
{
  m = Serial.parseInt(); // Присваивание глобальной переменной первое целочисленное значение
  n = Serial.parseInt();
  k = Serial.parseInt();
Serial.print(«X = « ); // Вывод координат, введённых пользователем
Serial.println(m);
Serial.print(«Y = « );
Serial.println(n);
Serial.print(«Z = « );
Serial.println(k);
full(m, n, k); // Расчет углов поворота для введенных координат
Serial.print(«Angle #1: « ); // Вывод угла поворота первого двигателя
Serial.println(theta1);
Serial.print(«Angle #2: « );
Serial.println(theta2);
Serial.print(«Angle #3: « );
Serial.println(theta3);
set_pos(theta1, theta2, theta3, 1000); // Поворот на вычисленный угол
}
}

Значения можно вводить через любые знаки препинания, главное в правильном порядке, и разделять их пробелом.

В результате, код управляющей программы будет выглядеть следующим образом:

cpp
#include <DynamixelWorkbench.h>
#if defined(__OPENCM904__)
#define DEVICE_NAME «3» //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP
#elif defined(__OPENCR__)
#define DEVICE_NAME «»
#endif
#define BAUDRATE  1000000
#define DXL_ID1    1
#define DXL_ID2    2
#define DXL_ID3    3
DynamixelWorkbench dxl_wb;
int f = 45;
int Rf = 100;
int Re = 210;
int e = 20;
float theta1, theta2, theta3, theta;
float y10, y2, y3;
float x1, x2, x3;
float m, n, k;
void coord(float a, float b)
{
  x1 = a;
  y10 = b;
  x2 = a*cos(2.094) + b*sin(2.094);
  y2 = b*cos(2.094) - a*sin(2.094);
  x3 = a*cos(2.094) - b*sin(2.094);
  y3 = b*cos(2.094) + a*sin(2.094);
}
void find_angle(float x, float y, float z)
{
   float C1 = -0.5 * 0.57735 * f; // f/2 * tg 30
   y -= 0.5 * 0.57735 * e;
   float  a = (x*x + y*y + z*z +Rf*Rf - Re*Re - C1*C1)/(2*z);
   float b = (C1-y)/z;
   float d = -(a+b*C1)*(a+b*C1)+Rf*(b*b*Rf+Rf);
    if(d < 0)
      Serial.println(«Error»);

  float yj = (C1 - a*b - sqrt(d))/(b*b + 1);
  float zj = a + b*yj;
  float angle = atan(-zj/(C1 - yj));
  theta = angle;
}
void full(float x0, float y0, float z0)
{
  float R1 = sqrt(x0*x0+y0*y0);
    if( R1>120 || z0 < -200 || z0 > -150)
      {
      Serial.println(«Error: Radius!»);
      theta1 = 0;
      theta2 = 0;
      theta3 = 0;
  }
else
{
    coord(x0, y0);
    find_angle(x1, y10, z0);
    theta1 = theta;
    find_angle(x2, y2, z0);
      theta2 = theta;
    find_angle(x3, y3, z0);
      theta3 = theta;
}
}
void set_pos( float p1 , float p2, float p3,int del)
// функция для задания угла поворота вала двигателя на манипуляторе в радианах
{
  dxl_wb.goalPosition(DXL_ID1,p1);
  dxl_wb.goalPosition(DXL_ID2,p2);
  dxl_wb.goalPosition(DXL_ID3,p3);
  delay(del);
}
void setup() {
 const char *log ;
  Serial.begin(57600);

  uint16_t model_number1 = 1;
uint16_t model_number2 = 2;
uint16_t model_number3 = 3;
dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
dxl_wb.ping(DXL_ID1, &model_number1, &log);
dxl_wb.ping(DXL_ID2, &model_number2, &log);
dxl_wb.ping(DXL_ID3, &model_number3, &log);
dxl_wb.jointMode(DXL_ID1, 50, 0, &log);
dxl_wb.jointMode(DXL_ID2, 50, 0, &log);
dxl_wb.jointMode(DXL_ID3, 50, 0, &log);
}
void loop() {
 if (Serial.available() > 0 ) {
  m = Serial.parseInt();
  n = Serial.parseInt();
  k = Serial.parseInt();
Serial.print(«X = « );
Serial.println(m);
Serial.print(«Y = « );
Serial.println(n);
Serial.print(«Z = « );
Serial.println(k);
full(m, n, k);
Serial.print(«Angle #1: « );
Serial.println(theta1);
Serial.print(«Angle #2: « );
Serial.println(theta2);
Serial.print(«Angle #3: « );
Serial.println(theta3);
set_pos(theta1, theta2, theta3, 700);
 }
}

4.5. Техническое зрение ​

Функционал Delta-манипулятора может быть расширен за счет применения модуля технического зрения TrackingCam, способного выполнять анализ объектов и различать их по цвету и форме.

4.5.1. Настройка модуля технического зрения TrackingCam. ​

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

Установленный модуль технического зрения на Delta-манипулятор

image210

Интерфейс настройки модуля технического зрения

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

Чтобы модуль мог распознать объект и передать контроллеру информацию о его положении, его нужно предварительно настроить. Для этого необходимо воспользоваться утилитой TrackingCam App, при помощи которой модулю можно задать цветовые и морфологические признаки объекта, настроить параметры порта ввода-вывода информации и сохранить настройки (см. учебное пособие «Техническое зрение роботов с использованием TrackingCam») (Рис. 4.10). Желательно выбрать объекты контрастных цветов и контрастный фон.

После сохранения настроек модуля перейдем к подключению.

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

image211

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

image213

image212

4.5.2. Система отсчета ​

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

Красным выделены оси камеры, черным – манипулятора.

Для начала узнаем координаты нулевой точки манипулятора для системы координат камеры. Поставим предмет, на который мы настроили модуль технического зрения, в центр (нулевая точка манипулятора). А в утилите TrackingCam App нажмем на окно, где отображается центр опознанного объекта. Таким образом, программа сообщит нам координаты этого объекта для камеры модуля – (X,Y). Модуль технического зрения выдает различные данные. В данном случае нас интересует лишь значения по оси Х и оси Y. Таким же способом можно узнать координаты объекта в другом положении на плоскости (S1 , S2).

Теперь осуществим перенос центр системы координат камеры в центр системы координат манипулятора и найдем координаты объекта в новой системе

где S1 и S2 - текущие координаты предмета

4.5.3. Программная реализация ​

Для взаимодействия с модулем технического зрения необходимо объявить его ID в начале программы

cpp
#define DXL_ID_C    51
Также объявим глобальные переменные для записи данных с модуля:
uint8_t Sen1;
uint16_t Sen2;
uint16_t Sen3;
uint16_t addr = 16;
float x, y;
float X1, Y1;
bool grab = false; // Служит для проверки на наличие объекта в поле зрения модуля

Далее добавим функцию по считыванию данных с модуля:

image214

Несовпадение осей ориентации камеры и манипулятора

Eqn33

cpp
void Cam()
{
    uint32_t resp[20]; // создание массива, в котором будут хранится данные
    int idx = 0;
    dxl_wb.readRegister(DXL_ID_CR, 16, 16, resp); // чтение значений

    Sen11 = resp[idx]+(resp[idx + 1]<<8); // присваиваем переменной значение

    idx += 2; // переход к следующему значению массива
   Sen21 = resp[idx] + (resp[idx + 1] << 8);
    idx += 2;
   Sen31 = resp[idx] + (resp[idx + 1] << 8);
}

Модуль возвращает 8 различных переменных, содержащих информацию о найденном объекте. Но в нашем случае достаточно использовать лишь первые три – порядковый номер предмета, настроенный в приложении TrackingCam App, и координаты по осям ОХ и ОY.

Теперь реализуем функции расчетов для модуля:

cpp
void CamCal()
{
float X = 150 - Sen2; // Нулевое значение для системы координат манипулятора
 float Y = 54 - Sen3;
X1 = -X;
 Y1 = -Y;
}

В функции setup() необходимо проинициализировать камеру, добавив такие строки:

cpp
dxl_wb.ping(DXL_ID_C);
Теперь можно рассмотреть функцию loop()
{
if(grab == false) // Изначально флаг grab имеет значение «False»
{
CamCal(); // Выполняется расчет координат объекта
Serial.println(«X & Y:» ); // Смотрим, в какие координаты перейдет манипулятор
Serial.println(X1 );
Serial.println(Y1 );
 x = X1; // Присваиваем координаты глобальным переменным
 y = Y1;
 grab = true; // Меняем значение переменной grab
}
else // Теперь переходим в новый цикл, где осуществляется переход в координаты объекта, его захват и освобождение его в нулевых координатах
{
full(x,y,-165);
set_pos(theta1, theta2, theta3, 1500);
full(0,0,-165);
set_pos(theta1, theta2, theta3, 1500);
grab = false; // Переход в цикл расчета координат нового объекта
delay(2000);
}

Таким образом, можно реализовать цикл отслеживания и захвата требуемого объекта при помощи Delta-манипулятора и модуля технического зрения TrackingCam.