Applied robotics инструкция по сборке манипулятора

Введение:

В данном уроке, двигаясь по шагам, мы соберём робот «Манипулятор».

Видео:

Для сборки нам понадобится крепеж:

	Наименование	Количество, шт.
1	Гайка М3	10
2	Винт М3х6	9
3	Винт М3х8	10
4	Винт М3х10	5
5	Винт М3х12	7
6	Винт М3х20	4

Шаг 1

Список деталей к Шагу 1

Номер позиции	Количество	Название
1	1	Основание
3	4	М3х20мм винт
4	4	М3 гайка
5	1	Опорная пластина
6	1	Крепление
7	1	Сервопривод
8	2	М3×8мм винт

Шаг 2

Список деталей к Шагу 2

Номер позиции	Количество	Название
1	2	М3 гайка
2	1	Крепление
3	1	Сервопривод
4	2	М3х8 винт
5	1	Основа левой руки
6	1	Параллельное крепление
7	1	Рычаг руки
8	1	М3×6мм винт
9	1	Серво рычаг
10	2	М3х12мм винт
11	1	Осевой серверный винт
12	1	Фиксирующий серверный винт

Шаг 3

Список деталей к Шагу 3

Номер позиции	Количество	Название
1	2	М3 гайка
2	1	Крепление
3	1	Сервопривод
4	2	М3х8 винт
5	1	Параллельное крепление
6	1	М3х6мм винт
7	1	Серво рычаг
8	2	М3×6мм винт
9	1	Рычаг правой руки
10	1	Основание правой руки
11	1	Осевой серверный винт
12	1	Фиксирующий серверный винт

Шаг 4

Список деталей к Шагу 4

Номер позиции	Количество	Название
1	1	Крепление вкладки левой руки
2	1	М3х6мм винт
3	1	Балка левой руки
4	1	Верхняя крышка
5	1	Двойной серво рычаг
6	2	Фиксирующий серверный винт.

Шаг 5

Список деталей к Шагу 5

Номер позиции	Количество	Название
1	2	М3 гайка
2	2	М3х12мм винт
3	1	Траверса основания манипулятора
4	1	Соединительное ребро жесткости

Шаг 6

Список деталей к Шагу 6

Номер позиции	Количество	Название
1	2	М3×6мм винт
2	1	Фиксирующий серверный винт
3	1	Балка левого запястья

Шаг 7

Список деталей к Шагу 7

Номер позиции	Количество	Название
1	1	Параллельная балка
2	1	М3х6мм винт
3	1	Коннектор
4	1	Балка правого запястья
5	1	Прокладка
6	2	M3x10 винт

Шаг 8

Список деталей к Шагу 8

Номер позиции	Количество	Название
1	1	Левый захват
2	1	Правый захват
3	1	Приводной рычаг
4	1	Левое крепление запястья
5	1	Правое крепление запястья
6	1	Нижнее крепление сервопривода
7	1	Верхнее крепление сервопривода.
8	1	Приводной рычаг
9	1	Осевой серверный винт.
10	1	Серво рычаг
11	1	Сервопривод
12	4	М3х8мм
13	3	М3х6мм
14	1	Фиксирующий серверный винт
15	1	М3х12мм винт
16	2	Прокладка

Шаг 9

Список деталей к Шагу 9

Номер позиции	Количество	Название
1	1	Прокладка
2	3	М3х10мм винт

Вы можете скачать данную инструкцию по ссылке: Сборка робота-манипулятора. Часть 1

В собранном виде:

Наиль Загидуллин

МБОУ СОШ № 2 с. Стерлибашево

Лабораторные работы на Arduino

Робототехника

Оглавление

Лабораторная работа № 1 Светодиод 2

Сборка элементов на плате 4

Лабораторная работа № 2 Управляемый «программно» светодиод 7

Лабораторная работа № 3 Управляемый вручную светодиод 10

Сборка элементов на плате 12

Лабораторная работа № 4.1 Пьезодинамик 13

Сборка элементов на плате 13

Лабораторная работа № 4.2 Управляемый пьезодинамик 15

Сборка элементов на плате 15

Лабораторная работа № 5 Фоторезистор 17

Сборка элементов на плате 19

Лабораторная работа № 6 Кнопка 20

Лабораторная работа № 7 Термистор 22

Лабораторная работа № 8 Синтезатор 24

Лабораторная работа № 9 Взаимодействие Arduino с семисегментным индикатором 25

Лабораторная работа № 10 Обмен данными Arduino с ПК 27

Лабораторная работа № 11 Дисплей LCD 12С интерфейс 28

Лабораторная работа № 12 Сервопривод 30

Лабораторная работа № 13 Шаговый двигатель 32

Лабораторная работа № 13 Двигатель постоянного тока 35

Лабораторная работа № 14 ИК-датчик и ИК пульт 37

Лабораторная работа № 15 Bluetooth модуль 40

Лабораторная работа № 16 Дальномер 42

Лабораторная работа № 17 Датчик скорости 44

Приложения 46

Работа с набором «Конструктор программируемых моделей инженерных систем». Первое подключение. 4

Звездные войны 46

Виртуальный тренажёр на сайте К. Полякова 49

Скетч «Светофор» на тренажёре К. Полякова 1

Скетч «Светофор» на Ардуино 7

Лабораторная работа № 1 Светодиод

Цель: Научиться работать с виртуальным тренажёром Arduino

1. Зайдите на сайт: https://www.tinkercad.com/joinclass/YXHUNU9GQUEQ¹

2. Авторизуйтесь используя один их ниже представленных аккаунтов

3. Выберите Цепи – Создать цепь

4. Введите название проекта Проект 1

5. Перетащите на рабочее поле Arduino Uno 3 и дайте ему имя Ардуино

6. Перетащите на рабочее поле Малую макетную плату (Breadbord) и дайте ему имя Плата

7. Перетащите на рабочее поле Светодиод (Led) и дайте ему имя Светодиод

8. Перетащите на рабочее поле Резистор (Resistor) и дайте ему имя Резистор, 220 Ω
   

   Поворот элемента соединитель

9. Поверните Резистор, выберите соединитель обычный
   Соедините элементы так, как показано на рисунке:
   

1. Нажмите кнопку Код. Из выпадающего списка выберите Текст.

2. Введите код программы (скетча), можно скопировать и вставить:

int led = 8;

void setup()

{

pinMode(led, OUTPUT);

}

void loop()

{

digitalWrite(led, HIGH);

delay(1000);

digitalWrite(led, LOW);

delay(1000);

}

1. Нажмите кнопку Начать моделирование (светодиод должен моргать)

2. Нажмите кнопку Остановить моделирование (светодиод перестает моргать)

Пояснение кода:

int led = 8;  //объявление переменной целого типа, содержащей номер порта к которому мы подключили второй провод

void setup()  //обязательная процедура setup, запускаемая в начале программы; объявление процедур начинается словом void

pinMode(led, OUTPUT); //объявление используемого порта, led — номер порта, второй аргумент — тип использования порта — на вход (INPUT) или на выход (OUTPUT)

void loop() //обязательная процедура loop, запускаемая циклично после процедуры setup

digitalWrite(led, HIGH); //эта команда используется для включения или выключения напряжения на цифровом порте; led — номер порта, второй аргумент — включение (HIGH) или выключение (LOW)

delay(1000); //эта команда используется для ожидания между действиями, аргумент — время ожидания в миллисекундах (1 с = 1000 мс)

Дополнительное задание. Запустите модель с новыми параметрами:

Измените частоту мигания светодиода с периодом 2 с; 0, 5 с

Переключите светодиод на пин 7 и отредактируйте код на int led = 7;  

Лабораторная работа № 1.1 Работа с набором «Конструктор программируемых моделей инженерных систем». Первое подключение.

1. Запустить на компьютере программу Arduino IDE

2. Подключите микроконтроллер через USB кабель к компьютеру

3. Выберите в программе нужную плату и порт. Инструменты – плата –
   Arduino Mega or Mega 2650. Инструменты – порт – СОМ 4 (цифра может быть другой)

void setup()

{

pinMode(led, OUTPUT);

}

void loop()

{

digitalWrite(led, HIGH);

delay(1000);

digitalWrite(led, LOW);

delay(1000);

}

1. Нажмите кнопку Загрузка. (программа попросит сохранить файл, сохраните, выбрав имя test)
   Компьютер должен быть подключен к Интернет! Разрешить брандмауэру выход программы в Интернет.

2. После загрузки скетча на плате начнет мигать светодиод

Лабораторная работа № 1.2 Сборка элементов на плате

Оборудование: Макетная плата, 2 провода папа-папа, светодиод, резистор на 220 Ом.

Общие контакты

GND

Pin 8

Сборка элементов на плате производится по схеме:

Верхний провод соединяет свободный конец светодиода и пин под номером 8 на плате

Нижний провод соединяет своболный конец резистора и контакт GDN (минус или земля)

Получается последовательная цепь.

Введите код в программу Arduino IDE
int led = 8;

void setup()

1. Сохраните программу под именем Lab1.2

2. Нажмите кнопку Загрузка.

3. После загрузки скетча светодиод начнет мигать

{

pinMode(led, OUTPUT);

}

void loop()

{

digitalWrite(led, HIGH);

delay(1000);

digitalWrite(led, LOW);

delay(1000);

}

Приложение к ЛР № 1.2

Загрузка Pin 8 GND

Если светодиод не горит, поменяйте контакты светодиода

Лабораторная работа 1.3 Проект «Светофор» на Ардуино

Код программы

int led_G = 7;

int led_Y = 6;

int led_R = 5;

void setup()

{

pinMode(led_R, OUTPUT);

pinMode(led_Y, OUTPUT);

pinMode(led_G, OUTPUT);

}

void loop()

{

//red

digitalWrite(led_R, HIGH);

delay(3000);

//===red-yellow

digitalWrite(led_Y, HIGH);

delay(2000);

digitalWrite(led_R, LOW);

digitalWrite(led_Y, LOW);

//===green

digitalWrite(led_G, HIGH);

delay(3000);

digitalWrite(led_G, LOW);

//==yellow

digitalWrite(led_Y, HIGH);

delay(2000);

digitalWrite(led_Y, LOW);

delay(1000);

}

Лабораторная работа № 2 Управляемый «программно» светодиод

Цель работы: понять принцип работы широтно-импульсной модуляции (ШИМ). Научиться использовать ШИМ в проектах на базе Arduino.

Широтно-импульсная модуляция (ШИМ) — это метод получения изменяющегося аналогового значения посредством цифровых устройств.

Для ШИМ используется функция analogWrite(nun, значение); где пин — номер контакта Arduino, значение — число от О до 255. Например, значение 153 будет соответствовать величине тока в 3 вольта, а 76 — 1,5 вольт. 255 – 5 вольт. 0 – 0 вольт.

Ход работы:

Соберите схему (см. рис). Напишите код скетча.

// ШИН int led = 6; void setup() { pinMode(led, OUTPUT); } void loop() { //3 Вольта analogWrite(led, 153); delay(1000); //1,5 Вольта analogWrite(led, 76); delay(1000); //5 Вольт analogWrite(led, 255); delay(1000); }

Светодиод должен мигать с переменной яркостью

Доп. Задание. Измените код так, чтобы

1. Изменилась скорость мигания светодиода

2. Изменилась яркость мигания светодиода

Источник: Лабораторная работа №4 «Широтно-импульсная модуляция» — Программирование микроконтроллера Arduino в информационно-управляющих системах (bstudy.net)

Лабораторная работа № 3 Управляемый вручную светодиод

Цель: Знакомство с устройством Потенциометра

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

Оборудование: Малая макетная плата, резистор (220 В), светодиод, потенциометр,

Соединители: удлинитель -1, провода: папа-мама-3, папа-папа -3)

Соберите схему:
Тип провода – Схема. После соединения измените цвет провода.

Земля

// даём имена пинов со светодиодом

// и потенциометром

int led =9

int pot= A0

void setup()

{

// пин со светодиодом — выход

pinMode(led, OUTPUT);

// пин с потенциометром — вход

pinMode(pot, INPUT);

}

void loop()

{

// объявляем переменную x

int x;

// считываем напряжение с потенциометра:

// будет получено число от 0 до 1023

// делим его на 4, получится число в диапазоне

// 0-255 (дробная часть будет отброшена)

x = analogRead(pot) / 4;

// выдаём результат на светодиод

analogWrite(led, x);

}

Код программы:

int led = 9;

int pot = A0;

void setup()

{

pinMode(led, OUTPUT);

pinMode(pot, INPUT);

}

void loop()

{

int x;

x = analogRead(pot)/4;

analogWrite(led, x);

}

Загрузите программу и покрутите ручкой потенциометра – яркость светодиода должна изменятся

Источник:
Arduino для начинающих. Урок 3. Подключение потенциометра | Занимательная робототехника (edurobots.ru)

Сборка элементов на плате

• Если светодиод не горит — поменяйте контакты светодиода

• Если от потенциометра идёт дым и запах отключите питание. Установите ручку потенциометра в среднее положение и повторите попытку

• У Потенциометра полярность крайних контактов (+ и -) произвольная

Лабораторная работа № 4.1 Пьезодинамик

Цель: Знакомство с работой пьезоэлемента

Источник:
Arduino для начинающих. Урок 6. Подключение пьезоэлемента | Занимательная робототехника (edurobots.ru)

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

1. Доп. Информация:
   Приложение: Звездные войны

Сборка элементов на плате

Пояснение кода:

int p = 3; //объявляем переменную с номером пина, на который мы

//подключили пьезоэлемент

void setup() //процедура setup

{

pinMode(p, OUTPUT); //объявляем пин как выход

}

void loop() //процедура loop

{

tone (p, 500); //включаем на 500 Гц

delay(100); //ждем 100 мс

tone(p, 1000); //включаем на 1000 Гц

delay(100); //ждем 100 мс

}

Код программы:

int p = 3;

void setup()

{

pinMode(p, OUTPUT);

}

void loop() //процедура loop

{

tone (p, 500);

delay(100);

tone(p, 1000);

delay(100);

}

Лабораторная работа № 4.2 Управляемый пьезодинамик

Цель: Получение звука переменной частоты с помощью потенциометра.

int buzzer_pin = 3;

int pot_pin = A0;

void setup()

{

pinMode(buzzer_pin, OUTPUT);

}

void loop() //процедура loop

{

int rotation, frequency;

rotation = analogRead(pot_pin);

frequency = map(rotation,0,1023,3500,4500);

tone (buzzer_pin, frequency,20);

}

Сборка элементов на плате

Лабораторная работа № 5 Фоторезистор

Цель: Знакомство работой фоторезистора

Фоторезистор — резистор, сопротивление которого зависит от яркости света, падающего на него. Фоторезисторы используются в робототехнике как датчики освещенности. Встроенный в робота фоторезистор позволяет определять степень освещенности, определять белые или черные участки на поверхности и в соответствие с этим двигаться по линии или совершать другие действия.

Оборудование: 6 проводов “папа-папа”, фоторезистор, светодиод, резистор на 220 Ом, резистор на 10 кОм

Схема подключения:

Код программы:

int led = 13; //переменная с номером пина светодиода

int ldr = 0; //и фоторезистора

void setup() //процедура setup

{

pinMode(led, OUTPUT); //указываем, что светодиод — выход

}

void loop() //процедура loop

{

if (analogRead(ldr)

//если показатель освещенности меньше 800, включаем светодиод

else digitalWrite(led, LOW); //иначе выключаем

}

Источник:

Arduino для начинающих. Урок 7. Подключение фоторезистора | Занимательная робототехника (edurobots.ru)

Сборка элементов на плате

Лабораторная работа № 6 Кнопка

Цель: управление светодиодом с помощью кнопки

Сегодня подключаем к ардуино кнопку и светодиод (при нажатой кнопке светодиод будет гореть, при отжатой — не гореть)

Код:

int button = 2;

int led = 8;

void setup() {

pinMode(led, OUTPUT);

pinMode(button, INPUT);

}

void loop(){

if (digitalRead(button) == HIGH) {

digitalWrite(led, HIGH);

}

else {

digitalWrite(led, LOW);

}

}

Источник:
Arduino для начинающих. Урок 2. Подключение кнопки | Занимательная робототехника (edurobots.ru)

Лабораторная работа № 7 Термистор

Цель: Изучить работу термистора

Терморезистор (термистор) — полупроводниковый прибор, электрическое сопротивление которого изменяется в зависимости от его температуры.

10 КОм

Код:

#define B 3950 // B-коэффициент

#define SERIAL_R 10000 // сопротивление последовательного резистора, 10 кОм

#define THERMISTOR_R 10000 // номинальное сопротивления термистора, 100 кОм

#define NOMINAL_T 25 // номинальная температура (при которой TR = 100 кОм)

const byte tempPin = A0;

void setup() {

Serial.begin(9600);

pinMode( tempPin, INPUT );

}

Монитор порта

void loop() {

int t = analogRead( tempPin );

float tr = 1023.0 / t — 1;

tr = SERIAL_R / tr;

Serial.print(«R=»);

Serial.print(tr);

Serial.print(«, t=»);

float steinhart;

steinhart = tr / THERMISTOR_R;

steinhart = log(steinhart);

steinhart /= B;

steinhart -= 1.0 / (NOMINAL_T + 273.15);

steinhart = 1.0 / steinhart;

steinhart += 273.15;

Serial.println(abs(steinhart));

Подключение термистора к arduino. — У Павла! (psenyukov.ru)

delay(1000);

}

Лабораторная работа № 8 Синтезатор

Цель: получение звуков с помощью встроенных кнопок

Pin 8

GND

Код:

#define buz_pin 8

#define first_pin 30 // общее количество клавиш

#define button_count 3

void setup()

{

pinMode(buz_pin, OUTPUT);

}

void loop()

{

for (int i = 0; i

{

int buttonPin = i + first_pin;

boolean buttonUp = digitalRead(buttonPin);

if (!buttonUp) {

int frequency = 400 + i * 50;

tone(buz_pin,frequency, 50);

delay(500);

}

}

}

Лабораторная работа № 9 Взаимодействие Arduino с семисегментным индикатором

Цель: Знакомство с работой семисегментного индикатора

Код: Обратный отсчёт

/ создать массив для хранения конфигурации выводов индикатора для цифр

int num_array[10][7] = { { 1,1,1,1,1,1,0 }, // 0

{ 0,1,1,0,0,0,0 }, // 1

{ 1,1,0,1,1,0,1 }, // 2

{ 1,1,1,1,0,0,1 }, // 3

{ 0,1,1,0,0,1,1 }, // 4

{ 1,0,1,1,0,1,1 }, // 5

{ 1,0,1,1,1,1,1 }, // 6

{ 1,1,1,0,0,0,0 }, // 7

{ 1,1,1,1,1,1,1 }, // 8

{ 1,1,1,1,0,1,1 }}; // 9

// объявление функции

void Num_Write(int);

void setup()

{

// установить режимы работы выводов

pinMode(2, OUTPUT);

pinMode(3, OUTPUT);

pinMode(4, OUTPUT);

pinMode(5, OUTPUT);

pinMode(6, OUTPUT);

pinMode(7, OUTPUT);

pinMode(8, OUTPUT);

}

void loop()

{

// цикл счетчика

for (int counter = 10; counter 0; —counter)

{

delay(1000);

Num_Write(counter-1);

}

delay(3000);

}

// эта функция записывает значения в выводы, подключенные к индикатору

void Num_Write(int number)

{

int pin= 2;

for (int j=0; j 7; j++)

{

digitalWrite(pin, num_array[number][j]);

pin++;

}

}

Источник: Взаимодействие Arduino с семисегментным индикатором (radioprog.ru)

Лабораторная работа № 10 Обмен данными Arduino с ПК

Цель: Изучить механизм обмена данными между ПК и микроконтроллером

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

Код:

void setup() {

  // Инициализация последовательного порта с указанием скорости обмена данными ( по умолчанию лучше использовать 9600 бод)

  Serial.begin(9600);

  // Устанавливаем таймаут (значение по умолчанию слишком велико)

  Serial.setTimeout(100);

}

void loop() {

  // Если поступили данные с ПК

  if (Serial.available() 0) {

    // Считываем полученные данные

    String command = Serial.readString();

    // Формируем ответ

    String response = «Command » + command + » is accepted!»;

    // Отправляем ответ ПК

    Serial.println(response); 

}

}

Источники:

Обмен данными между ПК и Arduino через последовательный порт (с примером на C#) | Стрелец Coder (streletzcoder.ru)

Arduino: Serial Monitor. Общаемся с компьютером (alexanderklimov.ru)

Лабораторная работа № 11 Дисплей LCD 12С интерфейс

Цель: Знакомство работой дисплея

Контакты — цвет проводов

GND – чёрный

5V — красный

SCL — оранжевый

SDA — жёлтый

Для работы понадобится библиотека LiquidCrystal_I2C. Нужно скачать (

https://iarduino.ru/file/134.html

) и распаковать содержимое архива в папку Arduinolibraries.

Код:

#include // библиотека для управления устройствами по I2C

#include // подключаем библиотеку

LiquidCrystal_I2C LCD(0x27,16,2); // присваиваем имя LCD для дисплея

void setup() {

LCD.init(); // инициализация LCD дисплея

LCD.backlight(); // включение подсветки дисплея

LCD.setCursor(3, 0); // ставим курсор на 1 символ первой строки

LCD.print(«I LOVE»); // печатаем сообщение на первой строке

LCD.setCursor(2, 1); // ставим курсор на 1 символ второй строки

LCD.print(«ARDUINO»); // печатаем сообщение на второй строке

Изменить работу программы так, чтобы она работала как

1)секундомер 2) калькулятор (считывает данные с клавиатуры и выводит на дисплей)

}

void loop() {

}
Источник:

Arduino подключение LCD 1602 I2C (xn--18-6kcdusowgbt1a4b.xn--p1ai)

Лабораторная работа № 12 Сервопривод

Цель: знакомство с работой сервопривода

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

Оборудование: сервопривод PDI-6221MG, блок питания

Подключить сервопривод к пину № 9 с помощью трехжильного шлейфа

Arduino Mega позволяет подключить до 48 сервоприводов.

Код:

#include //используем библиотеку для работы с сервоприводом

Servo servo; //объявляем переменную servo типа Servo

void setup() //процедура setup

{

servo.attach(9); //привязываем привод к порту 10

}

void loop() //процедура loop

{

servo.write(0); //ставим вал под 0

delay(2000); //ждем 2 секунды

servo.write(180); //ставим вал под 180

delay(2000); //ждем 2 секунды

}

Если возникает шум в работе, измените углы от 10 до 170 градусов

Источник:

1. Arduino для начинающих. Урок 4. Управление сервоприводом | Занимательная робототехника (edurobots.ru)

2. Что такое сервопривод (сервомотор) и как им управлять — Суперайс (supereyes.ru)

Лабораторная работа № 13 Шаговый двигатель

Цель: знакомство с работой шагового двигателя

Шаговый двигатель (stepper motor) предназначен для точного позиционирования или перемещения объекта на заданное количество шагов вала.

Оборудование: Шаговый двигатель, драйвер, провода

Код:

// порты для подключения модуля ULN2003 к Arduino

#define in1 7

#define in2 8

#define in3 9

#define in4 10

int dl = 5; // время задержки между импульсами

void setup() {

pinMode(in1, OUTPUT);

pinMode(in2, OUTPUT);

pinMode(in3, OUTPUT);

pinMode(in4, OUTPUT);

}

void loop() {

digitalWrite(in1, HIGH);

digitalWrite(in2, LOW);

digitalWrite(in3, LOW);

digitalWrite(in4, HIGH);

delay(dl);

digitalWrite(in1, HIGH);

digitalWrite(in2, HIGH);

digitalWrite(in3, LOW);

digitalWrite(in4, LOW);

delay(dl);

digitalWrite(in1, LOW);

digitalWrite(in2, HIGH);

digitalWrite(in3, HIGH);

digitalWrite(in4, LOW);

delay(dl);

digitalWrite(in1, LOW);

digitalWrite(in2, LOW);

digitalWrite(in3, HIGH);

digitalWrite(in4, HIGH);

delay(dl);

}

Источник:

Arduino шаговый двигатель 28byj-48 (stepper motor) » Ардуино Уроки (xn--18-6kcdusowgbt1a4b.xn--p1ai)

Лабораторная работа № 13 Двигатель постоянного тока

Цель: знакомство с двигателем постоянного тока

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

Оборудование: DC-мотор (2 шт), источник питания

Прижать в гнёзда зеленого цвета с помощью маленькой отвертки провода двигателя

После запуска программы двигатели будут вращаться в одну потом в другую сторону

Код:

#define M1_dir 45 //направление вращения 1 двигателя

#define M1_Speed 44 //скорость вращения 1 двигателя

#define M2_dir 47 //направление вращения 2 двигателя

#define M2_Speed 46 //скорость вращения 2 двигателя

void setup() {

pinMode(M1_dir, OUTPUT);

pinMode(M1_Speed, OUTPUT);

pinMode(M2_dir, OUTPUT);

pinMode(M2_Speed, OUTPUT);

}

void loop() {

digitalWrite(M1_dir, LOW);

analogWrite(M1_Speed, 150);

digitalWrite(M2_dir, LOW);

analogWrite(M2_Speed, 150);

delay(2000);

digitalWrite(M1_dir, HIGH);

analogWrite(M1_Speed, 150);

digitalWrite(M2_dir, HIGH);

analogWrite(M2_Speed, 150);

delay(2000);

analogWrite(M1_Speed, 0);

analogWrite(M2_Speed, 0);

}

Лабораторная работа № 14 ИК-датчик и ИК пульт

Цель: знакомство с работой ИК-датчика и пульта

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

Скачать и установить библиотеку IRremote

Красный провод — +5V

Оранжевый – GND (земля или минус)

Черный – пин 2

ИК датчик может принимать сигналы и из обычного пульта от телевизора

 С помощью программы будем принимать команды с пульта и выводить их в окно монитора. 

Код:

#include «IRremote.h»

IRrecv irrecv(2); // указываем вывод, к которому подключен приемник

decode_results results;

void setup() {

Serial.begin(9600); // выставляем скорость COM порта

irrecv.enableIRIn(); // запускаем прием

}

void loop() {

if ( irrecv.decode( &results )) { // если данные пришли

Serial.println( results.value, HEX ); // печатаем данные

irrecv.resume(); // принимаем следующую команду

}

}

Управление яркостью светодиода (пин 13) с помощью ИК- пульта

Из предыдущей программы определяем коды громкости пульта

20DF40BF и 20DFC03F

#include «IRremote.h»

IRrecv irrecv(2); // указываем вывод, к которому подключен приемник

decode_results results;

void setup() {

irrecv.enableIRIn(); // запускаем прием

}

void loop() {

if ( irrecv.decode( &results )) { // если данные пришли

switch ( results.value ) {

case 0x20DF40BF:

digitalWrite( 13, HIGH );

break;

case 0x20DFC03F:

digitalWrite( 13, LOW );

break;

}

irrecv.resume(); // принимаем следующую команду

}

Источник:

Ардуино: инфракрасный пульт и приемник | Класс робототехники (robotclass.ru)

Лабораторная работа № 15 Bluetooth модуль

Цель: знакомство с работой Bluetooth модуля

Bluetooth модуль HC-05 используется для дистанционного управления ардуино или передачи данных с телефонов и гаджетов на микроконтроллер.

Нужно установить на телефон (Android) приложение Bluetooth терминал

Оборудование: Bluetooth модуль HC-05, источник питания, провода папа-мама 4 шт

При загрузке скэтча нужно отключить от питания Bluetooth модуль

Подключение контактов:

Arduino	Bluetooth
Pin 1 (TX)	RXD
Pin 0 (RX)	TXD
GND	GND
5V	VCC

Напишем программу, которая будет с телефона управлять светодиодом (пин 13)

Код:

int val;

int LED = 13;

void setup()

{

Serial.begin(9600);

pinMode(LED, OUTPUT);

digitalWrite(LED, HIGH);

}

void loop()

{

if (Serial.available())

{

val = Serial.read();

// При символе «1» включаем светодиод

if (val == ‘1’)

{

digitalWrite(LED, HIGH);

}

// При символе «0» выключаем светодиод

if ( val == ‘0’)

{

digitalWrite(LED, LOW);

}

}

}

Подключение терминала:

1. Включаем Bluetooth на телефоне и ищем новые устройства

2. Находим в списке расстройств «HC-05» и подключаемся к нему.

3. Телефон спросит пин-код. необходимо ввести «1234» или «0000»

4. Запустить терминал

Источник:

Урок 15. Bluetooth модуль HC-06 подключение к Arduino. Управление устройствами с телефона. — Описания, примеры, подключение к Arduino (iarduino.ru)

Лабораторная работа № 16 Дальномер

Цель: знакомство с работой дальномера (датчик расстояния)

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

Работа модуля основана на принципе эхолокации. Модуль посылает ультразвуковой сигнал и принимает его отражение от объекта. Измерив время между отправкой и получением импульса, не сложно вычислить расстояние до препятствия.

Оборудование: Дальномер, провода папа-мама 4 шт

Подключение контактов:

Arduino	Дальномер
Pin 11	ECHO
Pin 10	TRIG
GND	GND
5V	VCC

Нужно установить библиотеку NewPing.h

Программа выводит на монитор расстояние до объектов в см

Код:

#include

#define TRIGGER_PIN 10

#define ECHO_PIN 11

#define MAX_DISTANCE 400

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {

Serial.begin(9600);

}

void loop() {

delay(500);

Serial.print(«Ping: «);

Serial.print(sonar.ping_cm());

Serial.println(«cm»);

}

Источник:

Ультразвуковой дальномер HC-SR04: подключение, схема и примеры работы [Амперка / Вики] (amperka.ru)

Лабораторная работа № 17 Датчик скорости

Цель: знакомство с работой датчика скорости

Модуль датчика оборотов двигателя предназначен главным образом для определения скорости вращения вала электродвигателя.

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

Так в данном случае измеряется скорость – под событиями здесь понимаются импульсы, полученные в результате срабатывания оптического датчика во время вращения диска с прорезями. Датчик состоит из светодиода и фототранзистора, который воспринимает наличие или отсутствие излучения светодиода.

Подключение контактов:

Arduino	Датчик
GND	GND
2	OUT
5V	VCC

Код:

int encoder_pin = 2; // импульсные сигналы от модуля

unsigned int rpm; // количество оборотов в минуту

volatile byte pulses; // количество импульсов

unsigned long timeold;

// количество импульсов на оборот

unsigned int pulsesperturn = 12;

void counter()

{

//обновление счета импульсов

pulses++;

}

void setup()

{

Serial.begin(9600);

pinMode(encoder_pin, INPUT);

//Прерывание 0 на цифровой линии 2

//Срабатывание триггера по спаду сигнала

attachInterrupt(0, counter, FALLING);

// Инициализация

pulses = 0;

rpm = 0;

timeold = 0;

}

void loop()

{

if (millis() — timeold = 1000) {

//Не обрабатывать прерывания во время счёта

detachInterrupt(0);

rpm = (60 * 1000 / pulsesperturn )/ (millis() — timeold)* pulses;

timeold = millis();

pulses = 0;

Serial.print(«RPM = «);

Serial.println(rpm,DEC);

//Перезагрузка процесса обработки прерываний

attachInterrupt(0, counter, FALLING);

}

}

Источник:

Arduino и модуль фотоимпульсного датчика скорости вращения двигателя » Digitrode.ru

Приложения

Звездные войны

Подключите к плате Пьезоэлемент (пьезопищалка) Звук воспроизводится функцией tone() Синтаксис tone(pin, frequency, duration) Параметры pin: номер порта вход/выхода, на котором будет генерироваться сигнал frequency: частота сигнала в Герцах duration: длительность сигнала в миллисекундах Чтобы остановить звук, используют функцию noTone(pin) Напишем скетч, который сыграет мелодию из фильма «Звездные войны». // Звездные войны int led = 8; void setup() { pinMode(led, OUTPUT); } void loop() { delay(2000); tone(led,392,200); delay(400); tone(led,392,200); delay(400); tone(led,392,300); delay(400); tone(led,311,200); delay(250); tone(led,466,200); delay(100); tone(led,392,200); delay(350); tone(led,311,200); delay(250); tone(led,466,200); delay(100); tone(led,392,200); delay(700); tone(led,587,200); delay(350); tone(led,587,200); delay(350); tone(led,587,200); delay(350); tone(led,622,200); delay(250); tone(led,466,200); delay(100); tone(led,369,200); delay(350); tone(led,311,200); delay(250); tone(led,466,200); delay(100); tone(led,392,200); delay(200); noTone(led); }

Применение циклов
byte led_R; // случайный бит

const int Pin_tone = 8; // номер порта зуммера

const byte COUNT_NOTES = 39; // Количество нот

int frequences[COUNT_NOTES] = {

392, 392, 392, 311, 466, 392, 311, 466, 392,

587, 587, 587, 622, 466, 369, 311, 466, 392,

784, 392, 392, 784, 739, 698, 659, 622, 659,

415, 554, 523, 493, 466, 440, 466,

311, 369, 311, 466, 392

};

int durations[COUNT_NOTES] = {

350, 350, 350, 250, 100, 350, 250, 100, 700,

350, 350, 350, 250, 100, 350, 250, 100, 700,

350, 250, 100, 350, 250, 100, 100, 100, 450,

150, 350, 250, 100, 100, 100, 450,

150, 350, 250, 100, 750

};

void setup() {

pinMode(13, OUTPUT); // Настраиваем контакт на выход

pinMode(Pin_tone, OUTPUT); // Настраиваем контакт на выход

}

void loop() {

for (int i = 0; i Цикл от 0 до количества нот

tone(Pin_tone, frequences[i], durations[i] * 2); // Включаем звук, определенной частоты

led_R = random(0, 254); // Генерируем случайное число от 0 до 254

analogWrite(13, led_R); // Зажигаем светодиод на случайно сгенерированную яркость

delay(durations[i] * 2); // Пауза для заданной ноты

noTone(Pin_tone); // Останавливаем звук

}

}

Источник:
Урок 14. Музыка Star Wars на Arduino и RGB цветомузыка? Работа с tone(); — Описания, примеры, подключение к Arduino (iarduino.ru)

Виртуальный тренажёр на сайте К. Полякова

Откройте веб-страницу http://kpolyakov.spb.ru/school/robotics/arduino/arduino.htm

Удалите программу из окна слева и скопируйте в это окно новую программу:

//Звездные войны

пока 1

{

звук(15,392)

ждать(200)

звук(15,392)

ждать(200)

звук(15,392)

ждать(200)

звук(15,311)

ждать(250)

звук(15,466)

ждать(100)

звук(15,392)

ждать(350)

звук(15,311)

ждать(250)

звук(15,466)

ждать(100)

звук(15,392)

ждать(700)

звук(15,587)

ждать(350)

звук(15,587)

ждать(350)

звук(15,587)

ждать(350)

звук(15,622)

ждать(250)

звук(15,466)

ждать(100)

звук(15,369)

ждать(350)

звук(15,311)

ждать(250)

звук(15,466)

ждать(100)

звук(15,392)

ждать(200)

нетЗвука(15)

ждать(2000)

}

Щёлкнув по кнопке запустить программу, выполните её

Скетч «Светофор» на тренажёре К. Полякова

//1 секунду: горит только красный светодиод;

//0,5 секунды: горят красный и жёлтый светодиоды;

//1 секунду: горит только зелёный светодиод;

//0,5 секунды: горит только жёлтый светодиод.

пока 1

{

пин[0] = HIGH

ждать(3000)

пин[1] = HIGH

ждать(2000)

пин[0] = LOW

пин[1] = LOW

пин[2] = HIGH

ждать(3000)

пин[2] = LOW

пин[1] = HIGH

ждать(2000)

пин[1] = LOW

}

Ресурсы:

1. https://appliedrobotics.ru/?page_id=670 — pdf версии методичек

2. https://youtu.be/PqqvmoaAzfU — Обзор модуля и интерфейса программы TrackingCam

3. http://arduino.ru/Guide/Windows — arduino справочник на русском

4. https://amperka.ru/page/arduino-ide — Arduino IDE Arduino IDE 1.8.16 (стабильная версия релиз от 06.09.2021)

5. https://kpolyakov.spb.ru/school/robotics/arduino.htm — Сайт К. Полякова. Раздел Робототехника

6. https://youtu.be/nrczO8tWJNg — Arduino с #0

7. https://youtu.be/bO_jN0Lpz3Q?list=PLfDmj22jP9S759DT250VVzfZs_4VnJqLa — Arduino с #0

8. http://edurobots.ru/kurs-arduino-dlya-nachinayushhix — Arduino уроки

9. https://wokwi.com/arduino/new?template=arduino-uno — виртуальный симулятор Arduino

10. https://www.tinkercad.com/things/e2gKm6XCRUK-brave-tumelo/editel?tenant=circuits — Тинкеркад (Tinkercad Circuits Arduino) – бесплатный, простой и одновременно мощный эмулятор Arduino

1 YXHUNU9GQUEQ – это код, который отправляет учащимся учителем. Учитель заранее на сайте регистрируется, выбрав роль преподавателя и создает класс, формирует код.

В далёком 2009 году я загорелся идеей постройки собственного промышленного робота, который мог бы делать что-то полезное (а именно — сортировать мелкие детали на конвейере). Сразу скажу, что робота я построил (результат вы видите на заглавном фото), а заодно, в качестве побочного продукта, написал небольшую статью о кинематике дельта-роботов на форуме TrossenRobotics — американского продавца наборов из деталей для роботов. Они как раз проводили в то время какой-то конкурс для авторов. Конкурс я, разумеется, не выиграл, но статья на английском осталась. Несколько раз я порывался перевести её на родной язык, однако завершить начатое удалось только сейчас.

Если вы хотите построить свою модель дельта-робота, или просто разобраться, как можно вывести кинематические формулы для этого типа роботов (не выходя при этом за рамки школьной программы по алгебре и геометрии) — добро пожаловать под кат. Для тех, кто не очень любит теорию, в конце статьи приведены примеры готового кода на языке C.

Что такое дельта-робот

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

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

В центре нижней платформы (8) крепится т.н. рабочий орган (в английском языке употребляют термин end effector) робота (9). Это может быть манипулятор, захватывающее устройство или, например, экструдер в случае 3D принтера. Дополнительно может использоваться ещё один двигатель (11), который обеспечивает вращение рабочего органа через штангу (14).

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

Формулировка задачи

Чтобы построить своего собственного дельта-робота, необходимо научиться решать две задачи. В первой ситуации нам известна позиция, в которую мы хотим переместить манипулятор нашего робота (например, мы хотим схватить печенье, которое находится на конвейере в точке с координатами (x, y, z). Для этого нам требуется определить величины углов, на которые мы должны повернуть двигатели, связанные с рычагами робота, чтобы установить его в правильное положение для захвата. Процедура определения этих углов называется обратной (в некоторых русскоязычных источниках употребляется слово «инверсной») кинематической задачей.
Во второй ситуации нам известны углы, на которые повёрнуты управляющие моторы робота (если мы используем сервомоторы, то углы легко можно узнать, считав показания с датчиков углов поворота), и мы хотим найти положение платформы робота в пространстве (например, чтобы скорректировать его позицию). Это — прямая кинематическая задача.

Формализуем обе задачи. И неподвижное основание робота, и его движущуюся платформу можно представить в виде равносторонних треугольников: на схеме ниже они закрашены зелёным и розовым цветами соответственно. Углы поворота рычагов робота относительно плоскости основания (они же — углы поворота моторов) обозначены как Ѳ₁, Ѳ₂ и Ѳ₃, а координаты точки Е₀, расположенной в центре подвижной платформы и в которой в реальной жизни будет закреплён манипулятор нашего робота — как (x₀, y₀, z₀).

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

• f_inverse(x₀, y₀, z₀) → (Ѳ₁, Ѳ₂, Ѳ₃) для решения обратной кинематической задачи и
• f_forward(Ѳ₁, Ѳ₂, Ѳ₃) → (x₀, y₀, z₀) для решения прямой кинематической задачи.

Обратная кинематика

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

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

Конструкция робота подразумевает, что рычаг F₁J₁ (см. рисунок ниже) может вращаться лишь в плоскости YZ, описывая при этом окружность радиусом r_f с центром в точке F₁ (именно в этом месте он прикреплён к двигателю). В отличие от F₁, в узлах J₁ и E₁ используются универсальные шарниры, благодаря которым плечо E₁J₁ может свободно вращаться относительно E₁, описывая сферу радиусом r_e с центром в точке E₁.

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

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

Нижняя платформа является равносторонним треугольником, центром которого является точка E₀(x₀, y₀, z₀). Значит, расстояние

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

Расстояние E₁E’₁=x₀, тогда, согласно теореме Пифагора,

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

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

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

Если раскрыть скобки и вычесть одно уравнение из другого, можно линейным образом выразить z-координату точки J₁ через y-координату, после чего, подставив её во второе уравнение, получим обычное квадратное уравнение относительно y, из двух решений которого выберем наименьшее (об этом мы говорили выше). А получив таким образом координаты точки J₁, найдём и угол

Все выражения получились достаточно простыми благодаря удачному выбору системы координат: плечо рычага F₁J₁ всегда движется в плоскости YZ, поэтому мы можем просто не учитывать координату x. Чтобы сохранить это преимущество при нахождении двух оставшихся углов Ѳ₂ и Ѳ₃, воспользуемся симметрией конструкции дельта-робота. Сначала повернём нашу систему координат на 120° против часовой стрелки в плоскости XY вокруг оси Z:

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

Прямая кинематика

Попробуем решить обратную задачу: теперь нам известны углы Ѳ₁, Ѳ₂ и Ѳ₃, и мы хотим найти координаты (x₀, y₀, z₀) точки E₀, расположенной в центре подвижной платформы нашего робота. Зная углы, мы легко можем найти координаты точек J₁, J₂ и J₃ (см. рисунок ниже). Плечи рычагов J₁E₁, J₂E₂ и J₃E₃ могут свободно вращаться вокруг точек J₁, J₂ и J₃ соответственно, образуя в пространстве три сферы с радиусами r_e.

Воспользуемся хитрым приёмом: сместим центры каждой из этих сфер из точек J₁, J₂, J₃ в плоскости XY в направлении оси Z, используя вектора смещения E₁E₀, E₂E₀ и E₃E₀ соответственно (на рисунке они показаны в виде красных стрелочек). После этого преобразования окажется, что все три сферы пересекаются в точке E₀, как показано на рисунке ниже:

Получается, что для определения координат (x₀, y₀, z₀) точки E₀ мы должны найти точку пересечения трёх сфер, радиусы и координаты центров которых нам известны. Иными словами, нам надо решить систему из трёх уравнений, описывающих трёхмерные сферы:

где (x_i, y_i, z_i) — координаты центров сфер J’₁, J’₂ и J’₃, которые можно найти следующим образом:

Ниже для сокращения записи я буду использовать в качестве координат точек J’₁, J’₂ и J’₃ обозначения (x₁, y₁, z₁), (x₂, y₂, z₂) и (x₃, y₃, z₃) соответственно. Также хочу обратить внимание, что x₁=0 (поскольку точка J’₁ находится в плоскости YZ). Получаем следующую систему уравнений:

Введем обозначение

и, вычтя из верхнего уравнения второе и третье, а также из второго третье, получим:

Вычитая из первого уравнения второе (сократив, таким образом, y) и из второго третье (сократив x), сможем выразить x и y через z:

Теперь, подставив x и y, выраженные через z, в уравнение для первой окружности (с центром в точке J’₁), получим:

Осталось решить это квадратное уравнение (стандартным образом, через дискриминант), чтобы найти z (мы помним, что надо выбирать наименьший из двух z!), а через него x и y.

Примеры исходного кода

Ниже приведены примеры функций для расчёта кинематики дельта-робота на языке С. Названия переменных соответствуют обозначениям, используемым в статье, углы theta1, theta2 и theta3 указываются в градусах.

// размеры робота
// (обозначения см. на схеме)
const float e = 115.0;     // сторона подвижной платформы
const float f = 457.3;     // сторона неподвижного основания
const float re = 232.0;
const float rf = 112.0;

// тригонометрические константы
const float sqrt3 = sqrt(3.0);
const float pi = 3.141592653;    // PI
const float sin120 = sqrt3/2.0;
const float cos120 = -0.5;
const float tan60 = sqrt3;
const float sin30 = 0.5;
const float tan30 = 1/sqrt3;

// прямая кинематика: (theta1, theta2, theta3) -> (x0, y0, z0)
// возвращаемый статус: 0=OK, -1=несуществующая позиция
int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) {
    float t = (f-e)*tan30/2;
    float dtr = pi/(float)180.0;

    theta1 *= dtr;
    theta2 *= dtr;
    theta3 *= dtr;

    float y1 = -(t + rf*cos(theta1));
    float z1 = -rf*sin(theta1);

    float y2 = (t + rf*cos(theta2))*sin30;
    float x2 = y2*tan60;
    float z2 = -rf*sin(theta2);

    float y3 = (t + rf*cos(theta3))*sin30;
    float x3 = -y3*tan60;
    float z3 = -rf*sin(theta3);

    float dnm = (y2-y1)*x3-(y3-y1)*x2;

    float w1 = y1*y1 + z1*z1;
    float w2 = x2*x2 + y2*y2 + z2*z2;
    float w3 = x3*x3 + y3*y3 + z3*z3;

    // x = (a1*z + b1)/dnm
    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

    // y = (a2*z + b2)/dnm;
    float a2 = -(z2-z1)*x3+(z3-z1)*x2;
    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

    // a*z^2 + b*z + c = 0
    float a = a1*a1 + a2*a2 + dnm*dnm;
    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

    // дискриминант
    float d = b*b - (float)4.0*a*c;
    if (d < 0) return -1; // несуществующая позиция

    z0 = -(float)0.5*(b+sqrt(d))/a;
    x0 = (a1*z0 + b1)/dnm;
    y0 = (a2*z0 + b2)/dnm;
    return 0;
}

// обратная кинематика
// вспомогательная функция, расчет угла theta1 (в плоскости YZ)
int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) {
    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
    y0 -= 0.5 * 0.57735 * e;       // сдвигаем центр к краю
    // z = a + b*y
    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
    float b = (y1-y0)/z0;
    // дискриминант
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
    if (d < 0) return -1; // несуществующая точка
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // выбираем внешнюю точку
    float zj = a + b*yj;
    theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
    return 0;
}

// обратная кинематика: (x0, y0, z0) -> (theta1, theta2, theta3)
// возвращаемый статус: 0=OK, -1=несуществующая позиция
int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
    theta1 = theta2 = theta3 = 0;
    int status = delta_calcAngleYZ(x0, y0, z0, theta1);
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2);  // rotate coords to +120 deg
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
    return status;
}

Используемая литература

Все ключевые идеи о кинематике дельта-робота я взял из работы проф. Paul Zsombor-Murray «Descriptive Geometric Kinematic Analysis of Clavel’s «Delta» Robot». Честно признаюсь, что моей математической подготовки не хватило, чтобы понять её до конца, поэтому многое пришлось выводить самому.

Заключение

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