В прошлой статье я рассказал как собирал манипулятор, и, как обещал, в этой статье я расскажу как его программировать. Не то, чтобы это было очень сложно, но в данной статье я расскажу одну достаточно интересную вещь.
Ну и само собой видео работы манипулятора ожидает вас в конце статьи.
Для начала подключим все это к Arduino.
Схема подключения простая:
Теперь приступим к программированию.
Так как все кроме привода, который поворачивает платформу имеют ограниченные углы поворота. Поэтому для начала определим диапазон углов для каждого сервопривода с помощью метода из данной статьи (скетч, где мы вводим через консоль угол).
По очереди погоняв каждый привод я определил диапазоны для свои моторов
- Платформа - 0-180
- Правый - 180-100
- Левый - 20-60
- Клешня - 70-100
Ни при каких условиях углы поворота для каждого сервопривода не должны превышать свои диапазоны.
Теперь перейдем к самому интересному - как вы знаете, сервоприводы поворачиваются очень быстро. Даже под нагрузкой. В следствии чего когда я запустил манипулятор двигался очень быстро, и из-за этого в буквальном смысле спрыгнул со стола. А когда я его придерживал, он, беря какой либо предмет, при повороте запускал его в стену. Такой поворот дел меня не устраивал.
Поискав информацию я понял что нужного нам метода в библиотеке Servo.h нет, а предлагаемые решения были запускать движение в цикле с таймаутом после каждой итерации.
Так как у нас 4 двигателя, я решил просто расширить класс Servo, и встроить в него установку скорости вращения двигателя.
Давно я уже не писал что-то более менее полезное на C++, да и вообще я позиционирую себя как php разработчик, но то, что я когда-то начал программировать с C++ мне очень помогло не особо заморачиваясь, расширить данный класс.
Вот собственно код - Класс Servo_extend расширяет класс Servo, а метод rotate заменяет метод write.
Код неплохо прокомментирован, но если у вас останутся вопросы, то спрашивайте в комментариях.
#include <Servo.h> class Servo_extend : public Servo { public: void rotate(int angle){ // если скорость минимальная, то метод rotate // дублирует метод write if(this->speed <= 1){ this->angle = angle; this->write(angle); return; } if(angle > this->angle){ for(this->angle; this->angle <= angle; this->angle ++){ this->write(this->angle); delay(this->speed); } } else { for(this->angle; this->angle >= angle; this->angle --){ this->write(this->angle); delay(this->speed); } } } // устанавливаем угол void setAngle(int angle){ this->angle = angle; this->write(angle); } // Устанавливаем скорость // чем меньше число тем выше скорость void setSpeed(int speed){ this->speed = speed; } private: int angle = 0, speed = 1; }; Servo_extend servo0; Servo_extend servo1; Servo_extend servo2; Servo_extend servo3; void setup(){ // Привязка моторчиков к пинам servo0.attach(7); //левый (20-60) servo1.attach(6); //клешня (70-100) servo2.attach(5); //низ (0-180) servo3.attach(4); //правый (180-100) // Задаем стартовый угол для всех двигателей // Если стартовый угол 0, можно не устанавливать servo0.setAngle(60); servo1.setAngle(70); servo3.setAngle(100); // Скорость для всех двигателей servo0.setSpeed(10); servo2.setSpeed(10); servo3.setSpeed(10); // клешня должна двигаться быстро. // Можно просто у клешни все методы rotate // заменить на write } void loop(){ // управляем по очереди всеми двигателями servo2.rotate(0); delay(1000); servo3.rotate(180); servo0.rotate(20); delay(2000); servo1.rotate(100); delay(1000); servo3.rotate(100); servo0.rotate(60); delay(1000); servo2.rotate(180); delay(1000); servo3.rotate(180); servo0.rotate(20); delay(2000); servo1.rotate(70); delay(1000); servo3.rotate(100); servo0.rotate(60); delay(1000); }
А то что получилось из всего этого вы можете увидеть на следующем видео
2016-10-13 14:22