Ардуино робот для начинающих
- Войдите на сайт для отправки комментариев
Втр, 08/05/2012 - 12:16
Собрал робота на Ардуино Уно с ИК дальномером. Так как являюсь новичком в програмировании и робототехнике, то и проект соответственно для начинающих.
Статья с кодом на него и описанием деталей для сборки blogs.servodroid.ru/blog/arduino/41.html
Не понял как вставить видео и отредактировать сообщение выкладываю ссылку на ютуб youtu.be/k5TQm1x-VE8
Написал алгоритм для поиска предметов.
Модернизировал немного робота на ардуино. Приделал две лапки. Так же в качестве эксперимента подцепил к нему МП3 с активной колонкой, чтобы он мог комментировать свои действия.
Алгоритм и описание выложил так же в блоге blogs.servodroid.ru/blog/arduino/41.html
Вот видео. www.youtube.com/watch
Класс!!!
Изменения: переписал программу для поиска предметов. Впереди установил две оптопары, в качестве датчиков поверхности. Теперь робот сам может убирать со стола. Голос роботу вырезал из мультфильма робот Wall-e (робот уборщик). Код откомментирован, но возможно не идеальный. . Так как опыт программирования не большой.
Вот видео http://www.youtube.com/watch?v=IYgUkcRdluA
Ну и сам код
#include <Servo.h> #include <LMotorShield.h> LMotorShield lms; int ledPin = 13; // Кнопка включения мп3 int ledPin1 = 12; // кнопка перемотки мп3 int i; int j; int p[18]; // массив, здесь будем записывать данные с ИК дальномера int gp,gp1,gp2,g; int b; void setup() { { Serial.begin(9600); lms.begin(LMS_SERVOS); } pinMode(ledPin,OUTPUT); pinMode(ledPin1,OUTPUT); pinMode(3,OUTPUT); // Motor A скорость pinMode(7,OUTPUT); // Motor A направление pinMode(11,OUTPUT); // Motor B скорость pinMode(8,OUTPUT); // Motor B направление pinMode(2,OUTPUT); // Motor A стоп, пуск pinMode(4,OUTPUT); // Motor B стоп, пуск } void Forward() { // Подпрограмма движения робота вперед digitalWrite(3,HIGH); digitalWrite(7,LOW); analogWrite(11,240);// здесь скоростью вращения колеса регулируем прямолинейность движения digitalWrite(8,LOW); } void Backward(){ // Подпрограмма движения робота назад digitalWrite(3,HIGH); digitalWrite(7,HIGH); digitalWrite(11,HIGH); digitalWrite(8,HIGH); } void motorStop(){ // Подпрограмма остановки мотора digitalWrite(4,HIGH); digitalWrite(2,HIGH); } void motorRun(){ // Подпрограмма запуска мотора digitalWrite(2,LOW); digitalWrite(4,LOW); } void Spin_Left(){ // Вращение в лево digitalWrite(11,HIGH); digitalWrite(8,LOW); digitalWrite(3,HIGH); digitalWrite(7,HIGH); } void Spin_Right(){ // Вращение в право digitalWrite(3,HIGH); digitalWrite(7,LOW); digitalWrite(11,HIGH); digitalWrite(8,HIGH); } void loop() { lms.servoWrite(2,15); // раскрываем лапки delay(100); lms.servoWrite(4,160); // раскрываем лапки delay(100); lms.servoWrite(1,0); delay(200); j=0; for (j=0;j<18;j++){ // для j от 0 до 18 lms.servoWrite(1,j*10); // вращение сервы по 10 градусов gp=analogRead(0); // считываем данные с ИК радара delay(50); p[j]=gp; // записываем это значение в массив Serial.println(p[j]); // выводим на экран gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); g=min(gp1,gp2); // выбираем минимальное значение из двух if (g<4){ // Если значение близко к 0, то отходим назад и разворачиваемся motorRun(); Backward(); delay(500); Spin_Left(); delay(600); motorStop(); } if (p[j]<160){ // если значение с дальномера меньше 150, (примерно 25 см.), то шаг вперед motorRun(); Forward(); delay (50); motorStop();} else { // Иначе digitalWrite(ledPin1, HIGH); // нажимаем кнопку перемотка delay(100); digitalWrite(ledPin1, LOW); // отпускаем кнопку перемотка delay(100); lms.servoWrite(1,90); // поворачиваем голову вперед delay(200); gp=analogRead(0); // считываем данные с ИК радара Serial.println(gp); while(gp<160){ // поворачиваемся, пока не заметим предмет motorRun(); if (j>9){ // если предмет с лева, то Spin_Left();// поворот в сторону объекта } else { // иначе поворот вправо Spin_Right(); } delay (10); motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } gp=analogRead(0); // считываем данные с ИК радара delay(50); while(gp<400){ //Двигаемся вперед пока предмет не будет на расстоянии 10 см. motorRun(); Forward(); // движение до объекта delay (50); // время движения motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } lms.servoWrite(1,0); // убираем голову в сторону, чтобы она не мешала захвату предмета и транспортировке delay(100); motorRun(); Forward(); // движение до объекта delay (700); // время движения motorStop(); lms.servoWrite(2,140); // захват первой лапой delay(500); lms.servoWrite(4,15); // захват второй лапой delay(100); gp2=(analogRead(2)); gp1=(analogRead(1)); g=min(gp1,gp2); while(g>4) // движение после захвата до конца плоскости. { gp2=(analogRead(2)); gp1=(analogRead(1)); Serial.println(gp2); g=min(gp1,gp2); motorRun(); Forward(); delay(5); motorStop();} lms.servoWrite(2,15); // раскрываем лапки delay(100); lms.servoWrite(4,160); // раскрываем лапки delay(100); digitalWrite(ledPin, HIGH); // нажимаем кнопку стоп delay(150); digitalWrite(ledPin, LOW); // отпускаем кнопку стоп delay(100); motorRun(); Backward(); delay(100); Spin_Right(); delay(600); } } }Клево!
А ты в конкурсе не желаешь поучаствовать?
в конкурсе не желаешь поучаствовать?
Как буду готов маякну.
tu plastik . Нуу пока это обычная тележка с колесами. Четыре мотор редуктора лишнее, аккумов надолго не хватит, ведь еще датчики навешивать, а они тоже потребители. Цены в магазине по вашей ссылке вполне приемлимы. Мне лично драйвер моторов калининградский больше нравиться, на нем сразу выведены разъемы под 4 сервы, есть функция электротормоза, и под него написана библиотека., хотя он и на 1 А.
Подпрограмму движения, в приведенном вами коде, проще занести в void setup(), а в void loop() уже ее вызывать, код будет намного меньше в дальнейшем при "навешивании датчиков".
Подпрограмму движения, в приведенном вами коде, проще занести в void setup(), а в void loop() уже ее вызывать, код будет намного меньше в дальнейшем при "навешивании датчиков".
Спасибо за ответ. В написании кода не селен. Поэтому мне даже не понятен ваш совет с void loop() если не трудно поисните.
Ок !!! Все понял разобрался сам. Евгений подскажите а почему при компилировании вашего кодаа у меня появляется ошибка ?
LMotorShield lms;
LMotorShield does not name a type
подскажите а почему при компилировании вашего кодаа у меня появляется ошибка ?
LMotorShield lms;
LMotorShield does not name a type
Потому что у вас не загружена библиотека для мотор шилда. Я использовал http://robocraft.ru/blog/arduino/657.html такой. Под него написана библиотека LMotorShield.h. В своем коде я ее использовал для управления сервами и использовал функцию электротормоза. Можно обойтись и без нее, нужнно будет только переписать команды на сервы и MotorStop, MotorRun.
Спасибо Евгений. А вы еще случаем не подскажете гдеможно взять библиотеку для серводвигателей. Задача такова.
- серва в положении ноль .
- две кнопки право лево
- жму вправо серва в право
- - жму лево соответственно влево.
не чего нежму останавливается в свободном положении .
заранее спасибо
гдеможно взять библиотеку для серводвигателей.
Дак прямо в arduino 1.0 есть библиотека серво, и примеры должны быть. sketch/import Library/servo
А какую роль играют функции
motorStop и motorRun?
Я так понимаю можно и так записать, суть та же?
void motorstop() { motor1.run(RELEASE); motor2.run(RELEASE); }Что эти функции обеспечивают? Можно ли без них обойтись?
motorStop это функция, реализованная на примененном моторшылде. Позволяет осуществить динамическое торможение, то есть торможение без инерции. Можно конечно и без нее, но как будет набегать погрешность неизвестно. Вот статья по нему
http://robocraft.ru/blog/arduino/657.html
Круто!
Спаибо зо хорошее описание.
А ссылку можете обновить?
Старая не открывается- "Статья с кодом на него и описанием деталей для сборки blogs.servodroid.ru/blog/arduino/41.html"
Хотелось бы посмотреть и схему.
Извеняюсь за поздний ответ. http://www.servodroid.ru/forum/48-608-1 Здесь описаны комплектующие на него, будут вопросы спрашивайте.
круто, как для новичка офигенно круто
если заменить с ИК на ультрозвук что там нужно поменять в коде подскажи ?
Извеняюсь за поздний ответ. http://www.servodroid.ru/forum/48-608-1 Здесь описаны комплектующие на него, будут вопросы спрашивайте.
если заменить с ИК на ультрозвук что там нужно поменять в коде подскажи ?
если заменить с ИК на ультрозвук что там нужно поменять в коде подскажи ?
Если пойти дальше, то окажется, что ультразвуковой датчик имеет гораздо более широкую диаграмму направленности, а потому снимать расстояние для 18 разных углов бессмысленно - достаточно 3-5.
Ну и желательно бы поправить ошибки в коде, например, если мы поворачиваем серву на некоторый угол, то следует СНАЧАЛА подождать, пока она повернется, и только ПОТОМ измерять расстояние, а не наоборот.
если заменить с ИК на ультрозвук что там нужно поменять в коде подскажи ?
Если пойти дальше, то окажется, что ультразвуковой датчик имеет гораздо более широкую диаграмму направленности, а потому снимать расстояние для 18 разных углов бессмысленно - достаточно 3-5.
Ну и желательно бы поправить ошибки в коде, например, если мы поворачиваем серву на некоторый угол, то следует СНАЧАЛА подождать, пока она повернется, и только ПОТОМ измерять расстояние, а не наоборот.
Слабо все это сделать для навичка
Главное - начать.
Приветствую всех. Попытался повторить робота-уборщика представленного здесь. Сразу скажу не хочет РОБОТяга работать как у автора. Естественно это моя первая попытка разобраться в программировании Ардуино и пару месяцев изучения темы не смогли внести ясность в понимании того что я делаю не так. Питание силовой части организовал отдельно от Ардуинки.
Немного изменил компоненты робота:
1. Вместо LMotorShield подключил движки к драйверу на L298N
2. Вместо ИК дальномера SHARP GP2D120 использовал SHARP GP2Y0A41SK аналогичный по техническим характеристикам (дальность от 4 до 30 сантиметров).
3. Подключал всё к Ардуино UNO для удобства через Arduino Sensor Shield v 5.0
Вот данные подключения:
Analog 0 - ИК радар Sharp
Analog 1 - 1-й ИК датчик поверхности
Analog 2 - 2-й ИК датчик поверхности
Digital 2 и 7 - мотор А вращение
Digital 3 - ШИМ мотора А
Digital 2 и 7 - мотор В вращение
Digital 3 - ШИМ мотора В
Digital 6 - серва головы
Digital 10 - серва левой лапки
Digital 9 - серва правой лапки
Вот скетч от автора, немного неумело переделанный мной под мою распиновку:
#include <Servo.h> Servo rightHand; Servo leftHand; Servo head; int IN1 = 7; // Input1 подключен к выводу 7 мотор А вращение int IN2 = 2; // Input2 подключен к выводу 2 мотор А вращение int IN3 = 8; // Input3 подключен к выводу 8 мотор B вращение int IN4 = 4; // Input4 подключен к выводу 4 мотор B вращение int ENB = 3; // ШИМ вторго мотора (левого) int ENA = 11; // ШИМ первого мотора (правого) int ledPin = 13; // Кнопка включения мп3 int ledPin1 = 12; // кнопка перемотки мп3 int i; int j; int p[18]; // массив, здесь будем записывать данные с ИК дальномера в разных его положениях int gp,gp1,gp2,g; int b; void setup() { { Serial.begin(9600); } rightHand.attach(9); // подключение правой лапки leftHand.attach(10); // подключение левой лапки head.attach(6); // подключение сервы головы pinMode(ledPin,OUTPUT); pinMode(ledPin1,OUTPUT); pinMode (ENA, OUTPUT); // мотор А скорость pinMode (IN1, OUTPUT); // мотор А вращение pinMode (IN2, OUTPUT); // мотор А вращение pinMode (ENB, OUTPUT); // мотор В скорость pinMode (IN3, OUTPUT); // мотор В вращение pinMode (IN4, OUTPUT); // мотор В вращение } void Forward() { // Подпрограмма движения робота вперед digitalWrite (IN1, HIGH); digitalWrite (IN2, LOW); digitalWrite (IN3, HIGH); digitalWrite (IN4, LOW); analogWrite(ENA,170); // подаем на вывод ENА и ENВ управляюший ШИМ сигнал analogWrite(ENB,170); } void Backward(){ // Подпрограмма движения робота назад digitalWrite (IN1, LOW); // вращаем мотор в обратную сторону digitalWrite (IN2, HIGH); digitalWrite (IN3, LOW); digitalWrite (IN4, HIGH); analogWrite(ENA,170); // подаем на вывод ENА и ENВ управляюший ШИМ сигнал analogWrite(ENB,170); } void motorStop(){ // Подпрограмма остановки мотора analogWrite(ENA,0); // подаем на вывод ENА и ENВ управляюший ШИМ сигнал равный нулю analogWrite(ENB,0); } void motorRun(){ // Подпрограмма запуска мотора analogWrite(ENA,170); // подаем на вывод ENА и ENВ управляюший ШИМ сигнал analogWrite(ENB,170); } void Spin_Left(){ // Поворот влево digitalWrite (IN1, LOW); digitalWrite (IN2, HIGH); digitalWrite (IN3, HIGH); digitalWrite (IN4, LOW); } void Spin_Right(){ // Поворот вправо digitalWrite (IN1, HIGH); digitalWrite (IN2, LOW); digitalWrite (IN3, LOW); digitalWrite (IN4, HIGH); } void loop() { rightHand.write(15); // раскрываем правую лапку delay(100); leftHand.write(160); // раскрываем левую лапку delay(100); head.write(90); // голова смотрит вперёд delay(200); j=0; for (j = 0; j < 18; j++){ // для j от 0 до 18 head.write(j*10); // вращение сервы по 10 градусов (умножаем полученное значение на 10) gp=analogRead(0); // считываем данные с ИК радара delay(50); p[j]=gp; // записываем это значение в массив Serial.println(p[j]); // выводим на экран gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); g=min(gp1,gp2); // выбираем минимальное значение из двух if (g>900){ // Если значение больше 900 (датчик сработал), то отходим назад и разворачиваемся motorRun(); Backward(); delay(500); Spin_Left(); delay(600); motorStop(); } if (p[j]<160){ // если значение с дальномера меньше 150, (примерно 25 см.), то шаг вперед motorRun(); Forward(); delay (50); motorStop();} else { // Иначе head.write(90); // поворачиваем голову вперед delay(200); gp=analogRead(0); // считываем данные с ИК радара Serial.println(gp); while(gp<160){ // поворачиваемся, пока не заметим предмет motorRun(); if (j>9){ // если предмет с лева, то Spin_Left(); // поворот в сторону объекта } else { // иначе поворот вправо Spin_Right(); } delay (10); motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } gp=analogRead(0); // считываем данные с ИК радара delay(50); while(gp<400){ //Двигаемся вперед пока предмет не будет на расстоянии 10 см. motorRun(); Forward(); // движение до объекта delay (50); // время движения motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } head.write(175); // убираем голову в сторону, чтобы она не мешала захвату предмета и транспортировке delay(100); motorRun(); Forward(); // движение до объекта delay (70); // время движения motorStop(); rightHand.write(80); // захват первой лапой delay(500); leftHand.write(80); // захват второй лапой delay(100); gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); g=min(gp1,gp2); // выбираем минимальное значение из двух while(g<100) // движение после захвата до конца плоскости. { gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); Serial.println(gp2); g=min(gp1,gp2); // выбираем минимальное значение из двух motorRun(); Forward(); delay(5); motorStop();} rightHand.write(15); // раскрываем правую лапку delay(100); leftHand.write(160); // раскрываем левую лапку delay(100); motorRun(); Backward(); delay(100); Spin_Right(); delay(600); } } }Теперь о грустном, о проблемах с которыми я столкнулся: Если отключить двигатели и двигать робота руками, то всё работает отлично. Он вертит головой, как увидит препятствие перестаёт "оглядываться", далее поворачиваешь его к нему и продвигаешь вперёд. Далее он отворачивает голову, захватывает лапами предмет и после дальнейшего его продвижения вперёд отпускает предмет у края стола.
Но если подключаешь двигатели, то робот начинает жить своей жизнью. После того как он увидит препятствие, он замирает (останавливается) вместо того, чтобы повернуться к предмету и подъехать к нему. Двигатели конечно продолжают работать, но как-то в полсилы и ему не хватает мощи для поворота.
Чаще всего просто не замечает никаких предметов, а проносится мимо до края стола (грешу на дальность ИК дальномера Sharp (от 4 до 30 сантиметров)), срабатывают датчики поверхности, он резко рвёт немного назад разворачивается. При повторном подъезде к краю он уже не останавливается а падает в пропасть....
Не стал смотреть код, поскольку ваше описание проблем полностью подпадает под понятие "разводка питания и помехи". :)
1. L298N "жрет" примерно 1-1.5в от батареек. Соответственно, для полноценного управления моторами ему надо поднять питание. Скажем так: 6-вольтовые моторые при питании через L298N от 6в .. "еде шевелятся" под нагрузкой. Соответсвенно, смотрите какие у вас моторы и повышайте питание с учетом потерь на драйвере.
2. "живет сам по себе" указывает на прохождение помех от двигателей в каналы управления Дуньки. В частности, возможны варианты:
а) помех от искрения коллекторов - лечится напаиванием кондесаторов примерно 10н х 25в непосредственно на коллектор двигателя. Лучше 3шт: между щетками и с каждой щетки на корпус двигателя.
б) Также возможно высокое сопротивление источника напряжения - аккумуляторов (если питание от них и общее с дунькой) .. в этом случае, включенный мотор просаживает питание ниже 5в на плате Дуньки. Лечится заменой источника питания или питанием дуньки от отдельного источника или сильной развязкой общего источника через добавление конденсаторов - парой электролит большой емкости.
У меня на входе стабилизатора стоит 1000.0х25в, а на шине +5в дополнительно напаяна пара 470.0х10в + 100н х 15в. Входной электролит поставлен за входным диодом от разъема внешнего питания и питание от аккумуляторов подается на этот разъем. При просадке аккумов от включения моторов диод блокирует разрядку входного конденсатора на моторы и держит напряжение на входе стабилизатора, а внутренняя пара конденсаторов блокирует просадки питания от управления серводвигателями (их у нас бывает до 12шт одноовременно) и снижает наводки по питанию внешних датчиков (дальности, препятствия, узв, аналоговые, цвета и т.д. .. до 6-8шт используется одновременно тоже). Это все на Мега2560 крутится ежели вчё.
я решал проще немного: 2 LI-ION аккумулятора последовательно с них питание на моторы, а с одного через повышалку на ардуину. просадка общего питания не влияет на ардуину
У меня 8 Ni-MH/батарейки по 1.5в, последовательно с "отводами". Ибо нам требуются напруги: 9.6(12)в - питание 9в моторов Лего и наших 12в моторов с энкодерами; 7.2(9.0)в - питание платы Мега2560; 6.0(7.5) - питание низковольтных и жрущих моторов (3-6в, токи под 2А) .. это же питание идет на серводвигатели. Если что-то не подключается, то и не используется и сказать заранее что просадит по питанию - сложно. Поэтому и сделана стабилизация и защита "со всехх сторон". :)
а смысл батарейки ставить? тем более с такими токами
Ну .. так-то он у нас катается на аккумуляторах. а вот на соревнования .. ставим батарейки. Не всегда и не везде есть возможность зарядить робота между заездами: то розеток нет, то зарядник один на всех, а вот заменить аккумуляторы/батарейки в перерыве - частенько разрешается. :)
Питание у меня идет от 4-х Li-Ion формата18650. Два на ардуинку и два на силовую часть. Кондёры на движки поставил сразу. А вот электролиты по питанию около 1000 мкф нужно добавить. Робот бегает бодренько, но после того, как он увидит цель, он впадает в ступор (я думаб, что дело именно в коде). Постараюсь снять видосик его поведения...
конечно в коде. ищи
вот видосик поведения робота при обнаружении предмета: http://youtu.be/N8ZDusqah-0
Ну ошибок в коде предостаточно. Попробуйте переписать код с отсутпами, и да, помните что значение в переменной не изменяется оттого, что Вы опращиваете её значение в цикле. И если чтение с датчика происходит ДО начала цикла и оно разрешает повторять цикл, то он точно "зациклится" .. вот такая тавтология.. :)
Ну ошибок в коде предостаточно. Попробуйте переписать код с отсутпами, и да, помните что значение в переменной не изменяется оттого, что Вы опращиваете её значение в цикле. И если чтение с датчика происходит ДО начала цикла и оно разрешает повторять цикл, то он точно "зациклится" .. вот такая тавтология.. :)
Дело в том, что код я целиком брал от автора статьи, изменив только пины подключения. И в написании я не силён. Я очень много не понимаю того, что там вообще написано... :-(
И что такое переписать код с отступами? Вроде аккуратно все, не?
Если не сложно ткните носом в ошибки в коде... Дочка ждёт когда робот наконец будет справляться со своими обязанностями.
// С отступами, это примерно так: #include <Servo.h> Servo rightHand; Servo leftHand; Servo head; // Все определения констант (номера ножек и пр. что не изменяется по ходу работы скетча) // правильно заменить на такое: // #define IN1 7 /* Input1 подключен к выводу 7 мотор А вращение */ // int IN1 = 7; // Input1 подключен к выводу 7 мотор А вращение int IN2 = 2; // Input2 подключен к выводу 2 мотор А вращение int IN3 = 8; // Input3 подключен к выводу 8 мотор B вращение int IN4 = 4; // Input4 подключен к выводу 4 мотор B вращение int ENB = 3; // ШИМ вторго мотора (левого) int ENA = 11; // ШИМ первого мотора (правого) int ledPin = 13; // Кнопка включения мп3 int ledPin1 = 12; // кнопка перемотки мп3 int i; int j; int p[18]; // массив, здесь будем записывать данные с ИК дальномера в разных его положениях int gp,gp1,gp2,g; int b; void setup() { Serial.begin(9600); rightHand.attach(9); // подключение правой лапки leftHand.attach(10); // подключение левой лапки head.attach(6); // подключение сервы головы pinMode(ledPin,OUTPUT); pinMode(ledPin1,OUTPUT); pinMode (ENA, OUTPUT); // мотор А скорость pinMode (IN1, OUTPUT); // мотор А вращение pinMode (IN2, OUTPUT); // мотор А вращение pinMode (ENB, OUTPUT); // мотор В скорость pinMode (IN3, OUTPUT); // мотор В вращение pinMode (IN4, OUTPUT); // мотор В вращение } /** * Подпрограмма движения робота вперед * --------------------------------------------------------------------------------------------------- * сначала выключаем ноги .. и только потом включаем, * дабы уменьшить нагрев драйвера от переходных токов * , затем подаем на вывод ENА и ENВ управляюший ШИМ сигнал */ void Forward() { digitalWrite (IN2, LOW); digitalWrite (IN4, LOW); delayMicroseconds(2); digitalWrite (IN1, HIGH); digitalWrite (IN3, HIGH); analogWrite(ENA,170); analogWrite(ENB,170); } /* * Подпрограмма движения робота назад * --------------------------------------------------------------------------------------------------- * аналогично, только наоборот.. */ void Backward() { digitalWrite (IN1, LOW); digitalWrite (IN3, LOW); delayMicroseconds(2); digitalWrite (IN2, HIGH); digitalWrite (IN4, HIGH); analogWrite(ENA,170); analogWrite(ENB,170); } /** * Подпрограмма остановки мотора * --------------------------------------------------------------------------------------------------- * подаем на вывод ENА и ENВ управляюший ШИМ сигнал равный нулю */ void motorStop() { analogWrite(ENA,0); analogWrite(ENB,0); } /** * Подпрограмма запуска мотора * --------------------------------------------------------------------------------------------------- * в текущем направлении! подаем только скорость НЕ меняя направления моторов! */ void motorRun() { analogWrite(ENA,170); analogWrite(ENB,170); } /** * Поворот влево * --------------------------------------------------------------------------------------------------- */ void Spin_Left() { digitalWrite (IN1, LOW); digitalWrite (IN4, LOW); delayMicroseconds(2); digitalWrite (IN2, HIGH); digitalWrite (IN3, HIGH); } /** * Поворот вправо * --------------------------------------------------------------------------------------------------- */ void Spin_Right() { digitalWrite (IN2, LOW); digitalWrite (IN3, LOW); delayMicroseconds(2); digitalWrite (IN1, HIGH); digitalWrite (IN4, HIGH); } // ================ Главный цикл ================ // // повторяем ЭТО все время, "без остановок": void loop() { rightHand.write(15); // раскрываем правую лапку delay(100); leftHand.write(160); // раскрываем левую лапку delay(100); head.write(90); // голова смотрит вперёд delay(200); // !!! итого имеем 0.4сек задержку на начало выполнения каждого повтора! ТАК НАДО?!? // !!! j=0; излишне, есть внутри for. for (j = 0; j < 18; j++){ head.write(j*10); // вращение сервы по 10 градусов (умножаем полученное значение на 10) gp=analogRead(0); // считываем данные с ИК радара delay(50); p[j]=gp; // записываем это значение в массив .. ?ЗАЧЕМ? Serial.println(p[j]); // выводим на экран (задержка порядка 50мсек) gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); g=min(gp1,gp2); // выбираем минимальное значение из двух if (g>900){ // Если значение больше 900 (датчик сработал), то отходим назад и разворачиваемся Backward(); motorRun(); delay(500); Spin_Left(); delay(600); motorStop(); } // !!! значение дальномера в данный момент лежит в gp .. ваще-та тоже. :) if (p[j]<160){ // если значение с дальномера меньше 150, (примерно 25 см.), то шаг вперед Forward(); motorRun(); delay (50); // !!! включение мотора на 50мсек .. точно "шаг"?!? motorStop();} else { // Иначе head.write(90); // поворачиваем голову вперед delay(200); gp=analogRead(0); // считываем данные с ИК радара !!! ещё раз .. Serial.println(gp); while(gp<160){ // поворачиваемся, пока не заметим предмет if (j>9){ // если предмет с лева, то Spin_Left(); // поворот в сторону объекта } else { // иначе поворот вправо Spin_Right(); } motorRun(); delay (10); // !!! это даже 1/5 предыдущего шага .. он точно тут "сдвинется с места"? motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } gp=analogRead(0); // считываем данные с ИК радара delay(50); while(gp<400){ //Двигаемся вперед пока предмет не будет на расстоянии 10 см. Forward(); // движение до объекта motorRun(); delay (50); // !!! время движения 50мсек .. шаг?!? motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } head.write(175); // убираем голову в сторону, чтобы она не мешала захвату предмета и транспортировке delay(100); motorRun(); Forward(); // движение до объекта delay (70); // время движения motorStop(); rightHand.write(80); // захват первой лапой delay(500); leftHand.write(80); // захват второй лапой delay(100); gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); g=min(gp1,gp2); // выбираем минимальное значение из двух while(g<100) // движение после захвата до конца плоскости. { gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); Serial.println(gp2); g=min(gp1,gp2); // выбираем минимальное значение из двух motorRun(); Forward(); delay(5); motorStop(); } rightHand.write(15); // раскрываем правую лапку delay(100); leftHand.write(160); // раскрываем левую лапку delay(100); motorRun(); Backward(); delay(100); Spin_Right(); delay(600); } } }Надеюсь так - читабельнее? :)
В целом можно конечно и так, что называется "в лоб" .. код условно-рабочий .. советую отрегулировать задержки под конкретно вашу ходовую часть: 10,50,70 миллисекунд, что указаны в операторах delay после включения и выключения моторов, скорее всего вам не подошли .. это чисто экспериментальные чиселки и их надо бы вытащить в константные определения #define тоже, дабы не искать по коду каждый раз при настройках.
А так .. код можно сказать "целиком ошибочен", ибо тут правильно применять автоматное программирование, а не "в лоб"..
Большое спасибо за потраченное время, отдельное спасибо за коментарии. С их помощью будет легче разобраться в коде. Как испробую исправленный Вами код, отпишусь как работает.
Э-э-э, а я разве что-то правил? :)
Э-э-э, а я разве что-то правил? :)
Переписал немного. А уже я на основе Ваших коментариев немного правил ...
Вот код, поправленный с помощью подсказок ув. Arhat109-2, за что ему отдельное спасибо. Теперь робот справляется со своими обязанностями на 99%. Немного не доворачивается в сторону обнаруженного предмета. Это наверное широкая диаграмма направленности датчика в горизонтальной плоскости виновата...
И так код:
#include <Servo.h> Servo rightHand; Servo leftHand; Servo head; #define IN1 7 /* Input1 подключен к выводу 7 мотор А вращение */ #define IN2 2 /* Input2 подключен к выводу 2 мотор А вращение */ #define IN3 8 /*Input3 подключен к выводу 8 мотор B вращение*/ #define IN4 4 /*Input4 подключен к выводу 4 мотор B вращение*/ #define ENB 3 /*ШИМ вторго мотора (левого)*/ #define ENA 11 /*ШИМ первого мотора (правого)*/ int i; int j; int p[18]; // массив, здесь будем записывать данные с ИК дальномера в разных его положениях int gp,gp1,gp2,g; int b; void setup() { Serial.begin(9600); rightHand.attach(9); // подключение правой лапки leftHand.attach(10); // подключение левой лапки head.attach(6); // подключение сервы головы pinMode (ENA, OUTPUT); // мотор А скорость pinMode (IN1, OUTPUT); // мотор А вращение pinMode (IN2, OUTPUT); // мотор А вращение pinMode (ENB, OUTPUT); // мотор В скорость pinMode (IN3, OUTPUT); // мотор В вращение pinMode (IN4, OUTPUT); // мотор В вращение } /** * Подпрограмма движения робота вперед * --------------------------------------------------------------------------------------------------- * сначала выключаем ноги .. и только потом включаем, * дабы уменьшить нагрев драйвера от переходных токов * , затем подаем на вывод ENА и ENВ управляюший ШИМ сигнал */ void Forward() { digitalWrite (IN2, LOW); digitalWrite (IN4, LOW); delayMicroseconds(2); digitalWrite (IN1, HIGH); digitalWrite (IN3, HIGH); analogWrite(ENA,150); analogWrite(ENB,150); } /* * Подпрограмма движения робота назад * --------------------------------------------------------------------------------------------------- * аналогично, только наоборот.. */ void Backward() { digitalWrite (IN1, LOW); digitalWrite (IN3, LOW); delayMicroseconds(2); digitalWrite (IN2, HIGH); digitalWrite (IN4, HIGH); analogWrite(ENA,100); analogWrite(ENB,100); } /** * Подпрограмма остановки мотора * --------------------------------------------------------------------------------------------------- * подаем на вывод ENА и ENВ управляюший ШИМ сигнал равный нулю */ void motorStop() { analogWrite(ENA,0); analogWrite(ENB,0); } /** * Подпрограмма запуска мотора * --------------------------------------------------------------------------------------------------- * в текущем направлении! подаем только скорость НЕ меняя направления моторов! */ void motorRun() { analogWrite(ENA,180); analogWrite(ENB,180); } /** * Поворот влево * --------------------------------------------------------------------------------------------------- */ void Spin_Left() { digitalWrite (IN1, LOW); digitalWrite (IN4, LOW); delayMicroseconds(2); digitalWrite (IN2, HIGH); digitalWrite (IN3, HIGH); } /** * Поворот вправо * --------------------------------------------------------------------------------------------------- */ void Spin_Right() { digitalWrite (IN2, LOW); digitalWrite (IN3, LOW); delayMicroseconds(2); digitalWrite (IN1, HIGH); digitalWrite (IN4, HIGH); } // ================ Главный цикл ================ // // повторяем ЭТО все время, "без остановок": void loop() { rightHand.write(15); // раскрываем правую лапку delay(25); leftHand.write(160); // раскрываем левую лапку delay(25); head.write(90); // голова смотрит вперёд delay(25); for (j = 0; j < 18; j++){ head.write(j*10); // вращение сервы по 10 градусов (умножаем полученное значение на 10) gp=analogRead(0); // считываем данные с ИК радара delay(5); p[j]=gp; // записываем это значение в массив Serial.println(p[j]); // выводим на экран gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); g=max(gp1,gp2); // выбираем максимальное значение из двух if (g>900){ // Если значение больше 900 (датчик сработал), то отходим назад и разворачиваемся Backward(); delay(800); motorRun(); Spin_Left(); delay(600); motorStop(); } if (p[j]<160){ // если значение с дальномера меньше 150, (примерно 25 см.), то едем вперед Forward(); motorRun(); delay (25); motorStop();} else { // Иначе head.write(90); // поворачиваем голову вперед delay(200); gp=analogRead(0); // считываем данные с ИК радара ещё раз Serial.println(gp); while(gp<160){ // поворачиваемся, пока не заметим предмет if (j>9){ // если предмет с лева, то Spin_Right(); // поворот в сторону объекта } else { // иначе поворот вправо Spin_Left(); // что-то напутал с подключением левого и правого мотора? так работает правильно } motorRun(); delay (100); motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } gp=analogRead(0); // считываем данные с ИК радара delay(50); while(gp<590){ //Двигаемся вперед пока предмет не будет на расстоянии 4 см. Forward(); // движение до объекта motorRun(); delay (25); motorStop(); gp=analogRead(0); // считываем данные с ИК радара delay(50); } rightHand.write(80); // захват первой лапой delay(300); leftHand.write(80); // захват второй лапой delay(100); gp2=(analogRead(2)); // считываем значение датчиков плоскости gp1=(analogRead(1)); g=max(gp1,gp2); // выбираем максимальное значение из двух while(g<900) // движение после захвата до конца плоскости. { gp2=(analogRead(2));// считываем значение датчиков плоскости gp1=(analogRead(1)); Serial.println(gp2); g=max(gp1,gp2); // выбираем максимальное значение из двух motorRun(); Forward(); delay(5); motorStop(); } rightHand.write(15); // раскрываем правую лапку delay(100); leftHand.write(160); // раскрываем левую лапку delay(100); motorRun(); Backward(); delay(400); Spin_Right(); delay(600); } } }Я убрал оттуда поворот головы в сторону перед захватом предмета (как у автора статьи) потому как серва с датчиком у меня не выходит вперёд за габариты робота и не мешает захвату.
Выражаю так же отдельную благодарность автору статьи Евгений74 за идею и исполнение.
Полезные роботы уборщики! )) Надеюсь, мой "лентяй" скоро "научится" делать что то полезное, так же, как ваши. Вот мой: https://youtu.be/AIZ3olAV26g