Arduino Robot 2WD помогите собрать с нуля
- Войдите на сайт для отправки комментариев
Втр, 01/03/2016 - 22:15
И так всем привет я новичек в этом деле помогите чем сможете.
Купил данный набор теперь не знаю как правильно собрать данную модель
Проси инструкцию у продавца
Вот тебе схема сборки платформы
Всю механику я собрал, платы накинул осталось подключить провода и запустить
Всю механику я собрал, платы накинул осталось подключить провода и запустить
Кто или что мешает?
Не знаю как правильно подключить провода и какой Скетч закачать на плату ардуино.
tes66,
я уверен в том, что Вы купили эту игрушку именно с целью собрать самостоятельно. Если бы это было не в кайф - купили бы готовую машинку и не парились бы, так же? Так и собирайте, ничего там сложного нет.
На платах (модулях) написаны их названия (их даже на катринке видно). По этим названиям гуглите и находите схемы включения - Вы же не думаете, что Вы первый, кто это собирает. Схем навалом - не найти невозможно.
Там же можете найти и скетчи, но это неинтересно. Весь смысл робота в том, чтобы его самому запрограммировать. Но, если не умеете, скетчи для каждого из Ваших модулей Вы легко найдёте (опять же названия модулей на них написаны). Пробуйте и не сдавайтесь и всё получится.
Недавно с детьми такой собирал. Там перемычек нехватает. Придётся заказывать или делать. Ссылка на скетч под такой же комплект - http://mysku.ru/blog/aliexpress/19132.html Компилировал под Arduino IDE 1.0.5
Сейчас буду разбиратся.
Кнопку я не подключал, забыл :-(, работает и без неё.
Кнопку я не подключал, забыл :-(, работает и без неё.
Я единственное не понял как определить в коде ПИНы подключения моторов через Arduino Sensor Shield V5.0
А они просто Ардуино перетащены, т.е. Сенсор шилд - просто разъёмы для датчиков, Жки иб.д. Можно обойтись и без неё. всё равно только Серво нормально подключается ( своим разъёмом), а всё остальное отдельными проводками.
Правильно ли написан код для моторов ???
как правильно указать в коде подключение по моим параметрам в схеме на плате ?
struct MOTOR // структура для хранения номеров pin-ов, к которым подключены моторчики { int in1; // INPUT1 int in2; // INPUT2 int enable; // ENABLE1 }; int Trig = 11; // УЗИ int Echo = 10; // УЗИ int ledPin = 13; // светодиод на плате ардуино unsigned int impulseTime = 0; // импульс для УЗИ unsigned int distance_sm = 0; // определяем порты, к которым подключены моторчики MOTOR MOTOR1 = { 7, 8, 9 }; MOTOR MOTOR2 = { 2, 4, 6 }; void setup() { Serial.begin(9600); pinMode(MOTOR1.in1, OUTPUT); // настраиваем выводы pinMode(MOTOR1.in2, OUTPUT); // на ВЫВОД pinMode(MOTOR2.in1, OUTPUT); pinMode(MOTOR2.in2, OUTPUT); pinMode(Trig, OUTPUT); // выход УЗИ pinMode(Echo, INPUT); // вход УЗИ pinMode(ledPin, OUTPUT); } void loop() { digitalWrite(Trig, HIGH); /* Подаем импульс на вход trig дальномера */ delayMicroseconds(10); // равный 10 микросекундам digitalWrite(Trig, LOW); // Отключаем impulseTime = pulseIn(Echo, HIGH); // Замеряем длину импульса distance_sm = impulseTime / 58; // Пересчитываем в сантиметры Serial.println(distance_sm); // Выводим на порт delay(100); if (distance_sm < 15) // Если расстояние менее X сантиметров { forward1() ; back2() ; } else { forward1() ; forward2(); } delay(100); } void forward1() // первый вперёд { digitalWrite(MOTOR1.in1, HIGH); digitalWrite(MOTOR1.in2, LOW); for (int i = 0; i <= 255; i++) analogWrite(MOTOR1.enable, i); } void forward2() // второй вперёд { digitalWrite(MOTOR2.in1, HIGH); digitalWrite(MOTOR2.in2, LOW); for (int i = 0; i <= 255; i++) analogWrite(MOTOR2.enable, i); } void back1() // первый назад { digitalWrite(MOTOR1.in1, LOW); digitalWrite(MOTOR1.in2, HIGH); for (int i = 0; i <= 255; i++) analogWrite(MOTOR1.enable, i); } void back2() // второй назад { digitalWrite(MOTOR2.in1, LOW); digitalWrite(MOTOR2.in2, HIGH); for (int i = 0; i <= 255; i++) analogWrite(MOTOR2.enable, i); }Нашел еще пару ошибок которые совершил при сборке платы, теперь поменял код на этот серво привод работает с сонаром отлично Но МОТОРЫ не едут в чем причина Помогите
int SlPin=12; //назначение входного пина на который вешается кнопка запуска основного алгоритма int EchoPin=10; //Echo-пин на УЗ-датчике int TrigPin=9; //Trig-пин на УЗ-датчике float DIST=0; //Дистанция int CRS=0; //Trig-пин на УЗ-датчике int ServoPin=11; //Сигнальный контакт сервопривода int POS=0; //Необходимое значение угла поворота. int OLDPOS=0; //Предыдущее значение угла поворота. //далее идет назначение номеров выводов в соответствии с //подключением к плате управления двигателями int ENA=5; int IN1=6; int IN2=7; int ENB=3; int IN3=4; int IN4=2; //установка максимального и минимального значения ШИМ //для ограничения напряжения питания двигателей. //минимальные значения подбираем опытным путем, //необходимо чтобы при этих значениях машинка смогла двигаться int MinPWML=0; int MinPWMR=0; int MaxPWML=150; int MaxPWMR=125; int CD[9]; int DT=0; void setup() { pinMode(SlPin, INPUT); digitalWrite(SlPin, HIGH); pinMode(13, OUTPUT); pinMode(ServoPin, OUTPUT); pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); pinMode(TrigPin, OUTPUT); pinMode(EchoPin, INPUT); digitalWrite(EchoPin, LOW); } void loop() {//Проверка нажатия кнопки исполнения алгоритма. if(digitalRead(SlPin)) { // --------------------------------- // Начало кода основной программы. // --------------------------------- DIST=sonar(); //Serial.print("DIST = "); //Serial.println(DIST); if (DIST<20){ roll('stp'); roll('bck'); delay(200); roll('stp'); CRS=course(); rotate(CRS); ServoPos(90); }else{roll('fwd');;} // -------------------------------- // Конец кода основной программы. // -------------------------------- }else{ OLDPOS=0; roll('stp'); ServoPos(90); } } //---------------------------------------------- // Процедуры управления движением конструкции. //---------------------------------------------- void steer(int l, int r) //установка направления вращения колес и скорости { //задаем направление движения колес if(l>0) { digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); }else{ digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); } if(r>0) { digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); }else{ digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); } //рассчитываем абсолютные значения параметров скорости l=abs(l); r=abs(r); //ограничиваем значения входных параметров if(l>254){l=254;} if(r>254){r=254;} //подгоняем значения входных параметров под нужный диапазон l=map(l, 0, 254, MinPWML, MaxPWML); r=map(r, 0, 254, MinPWMR, MaxPWMR); // ограничиваем значения входных параметров, иначе при их низких значениях // на двигатели будет подаваться ток меньше, чем требуется для обеспечения // вращения колес. if(l<=MinPWML){l=0;} if(r<=MinPWMR){r=0;} analogWrite(ENA, l); analogWrite(ENB, r); } void roll(char c) //указываем направление движения. { switch(c) { case 'fwd': steer(127, 127); break; case 'bck': steer(-127, -127); break; case 'stp': steer(0, 0); break; } } void rotate(int a) //поворот конструкции на заданный угол { int t=0; int s=254; //скорость вращения колес. if (a<90) { t=(90-a)*4,16; //Число 4,16 - время поворота конструкции на 1 градус при данных аргументах steer(), подобрано опытным путем. steer(-s, s); delay(t); steer(0, 0); }else{ t=(a-90)*4,16; steer(s, -s); delay(t); steer(0, 0); } } //-------------------------------------- // Процедуры управления сервоприводом. //-------------------------------------- void ServoPos(int angle) { //Функция выставления движка сервопривода на заданный угол. //Параметр-значение угла. int PD; //длительность управляющего импульса PD=2560-(angle)*11,11; //Производим рассчет количества циклов позиционирования. float DAN; //Разность предыдующего и устанавливаемого значений угла. DAN=OLDPOS-angle; DAN=abs(DAN); OLDPOS=angle; int CP; //Количество циклов позиционирования CP=DAN/1.8; //Оптимальное значение 1.8. Подбирается опытным путем. //посылаем управляющие импульсы //Обеспечиваем сервопривод работой в зависимости //от количаства циклов позиционирования. for(int i=0; i<CP; i++) //i=100 for 180 degree { digitalWrite(ServoPin, HIGH); delayMicroseconds(PD); digitalWrite(ServoPin, LOW); delayMicroseconds(23000); } } //------------------------------------------ // Процедуры работы с датчиком расстояния. //------------------------------------------ float sonar() //Измерение расстояния до обьекта. { int cnt=0; //Значение счетчика количества запросов. 2 запроса. float c1=0; //Переменные для хранения результатов замеров. float c2=0; repson: cnt++; //Посылаем стартовый импульс 10 мкс. digitalWrite(TrigPin, HIGH); delayMicroseconds(10); digitalWrite(TrigPin, LOW); //Принимаем ответ float c=0; for(long t=0; t<40000; t=t+10) { if(digitalRead(EchoPin)){c=c+10;} delayMicroseconds(10); } if(cnt==1){c1=c; goto repson;} if(cnt==2){c2=c;} if(abs(c1-c2)>35000){c=38000;}else{c=(c1+c2)/2;} //Рассчитываем расстояние. float d=0; d=2*c/58; return d; } int course() { int d=0; int dt=0; int cs=0; int p=0; for (p=0; p<7; p++) { ServoPos(p*30); dt=sonar(); if (dt>d){d=dt; cs=p*30;} } return cs; }При проверке выдает ошибку warning: case label value exceeds maximum value for type [enabled by default]
При проверке выдает ошибку warning: case label value exceeds maximum value for type [enabled by default]
А полностью сообщение привести религия не позволяет? Там номер строки был, а так мне приглось искать где это у Вас. Оно мне надо?
В одинарных кавычках пишется символ. Одиночный символ, а не три символа, как у Вас. Эта ошибка разбросана по всей программе, во могих местах.
В строке 149 с имеет тип char (т.е. как раз "одиночный символ"), а Вы рытаетесь сравнивать её со странноватой конструкцией 'stp'.
А питание на драйвер двигателей подвели? По фото видно что отсутствует.
Спасибо за ответ все поправил
Ну и остальной код естественно тоже подправил теперь не на что не ругается.
Подключение питания от платы Arduino Shield 5.0 к плате драйвера двигателей
Правильно ли я подключил питание ???
Может лучше от батарей на прямую ?
По сути индикатор питания 5v на плате драйвера горит, сигналы подаются, но они сами двигатели пищат и не более того.
Имелось в виду от батарей на драйвер двигателей. Когда собирал свой, добавил к отсеку на 4 батарейки ещё один на 2 шт. Итого 9в. Помнится мне что стабилизатор 7805 для нормальной работы требует не менее 7,5в. (в корпусе ТО-220, в смд может меньше - лень было искать и смотреть даташит)
Попробуйте поднять напряжение питания, или посмотреть строки 133-141, там как раз ограничение параметров выхода на движки
Ура заработало !!! Сейчас взял подпаял еще 2 провода от батарей и подключил их напрямую к драйверу двигателей и вуаля движки закрутились !
Добавлю скеч едет он на нем корявенько конечно даже не знаю как его заставить ехать более точно
При этом скече он постоянно сканирует сонаром и еле едет, подозрение падает на сильную чуствительность самого сонара который не дает нормально ехать роботу
Подскажите что не так я написал в коде ?
https://github.com/tes66/Arduino-Uno/blob/master/robot_2wd_servo_sonar.ino
Посмотрел скетч, мелкие замечания:
1. Управление драйвером моторов: первым делом ВЫКЛЮЧАЕМ не нужный пин, делаем задержку в 2мкс. и только потом включаем тот, куда хотим вращать моторы. Этот порядок позволяет исключить сквозные токи через драйвер и улучшить время жизни батареек. Заодно драйвер будет меньше греться.
2. Строки 140,141: почему установка идет в 0, а не в MINPWM..?
3. MAXPWM в районе 150 это 150/255 от реальной максимальной скорости моторов. Поиграйтесь с этим параметром.
4. Аналогично со значениями минимальной скорости. Моторы "заводятся" начиная с некоторого значения ШИМ и это далеко не 0. В среднем для хорошего мотора и редуктора с небольшими потерями это 10-20%, то есть в единицах 8-и битного ШИМ это примерно "от 50", да и сильно зависит от "материала трассы". У вас скорость выставляется в 127 .. это запросто может оказаться "ни о чем". Сделайте простой тестовый скетч: установка обоих моторов в некий ШИМ и проезд ровно 1 сек. и замерьте с какого значения ШИМ ваша конкретная тележка начинает движение хоть с какой-то скоростью, и сколько на каком значении ШИМ она проезжает за 1 сек. Такие "пробные" скетчи проще всего загонять в setup при пустом loop - однократное исполнение - гарантировано. У нас такой блок прошит прямо в setup() и просто комментарится, когда не нужен. При разной состоянии батареек - могут быть очень разные значения констант минимальной и максимальной скорости.
5. Ваш rotate() делает не "поворот", а "разворот на месте" за счет вращения колес в разные стороны. Попробуйте поворот на одном колесе или радийсные повороты за счет разной скорости колес в ОДНУ сторону... откроете для себя много интересного.
6. При обнаружении препятствия ваша тележка делает попытку крутить моторы "взад" 0.2сек .. при ваших параметрах, она реально должна успеть не только встать колом, но ещё и дернуться назад .. последующий разгон до небольшой скорости .. вот и вся причина. Время торможения - сильно зависит от материала трассы и текущей скорости движения. И такие АБС методы полезны на скользких трассах и реальных скоростях движения от 30см/сек при общем весе тележки в 500-600гр. По крайней мере по нашему опыту. В общем, это место - исключительно экспериментально.
Спасибо за исчерпывающий ответ, буду старатся исправить свой код.
Здравствуйте, tes66, внимательно следил за Вашими мытарствами, ожидая своих....вот и пришло время, посему прошу - опубликуйте ссылку на готовый скетч, пожалуйста...
Пост №24 - практически готовый скетч. На самом деле он у каждого свой, там нет ничего сложного или сверххитрого. Пробуйте самостоятельно по очереди: управление мотором, прием данных с датчиков, постепенно соединяя все в одно целое .. получите массу удовольствия, чем залить чужое и с удивлением создавать очередные посты тут "почему оно не работает".
Здравствуйте Arhat109-2, дело в том, что с этим скетчем я знаком задолго до открытия данной темы в форуме. Мои дети (я в селе веду кружок на разные технические темы) собрали ардуино - машинку как данная, только 4 wd, и где то с октября именно с данным скетчем экспериментировали, я особенно не вникал, т.к. в основном работаем с NXT. Но решили попробовать заявить ее на краевую олимпиаду по робототехнике в номинации Лабиринт http://robot.uni-altai.ru/meropriyatiya/olimpiady/ochnye-olimpiady/vi-olimpiada/zadaniya-lego-mindstorms/zadanie-2 и тут появились проблемы с использованием данного скетча, т.к. другие из сети не годились, а самим написать еще пока невозможно. Это замечательно, что Вы откликнулись, и, думаю, что сможете нам как то помочь, может быть, у Вас уже есть готовая программа. Мы не ставим своей целью где то победить, просто хотели показать возможности ардуино и немного ее порекламировать, т.к. в радиусе 300 км этим в школах практически никто не занимается. Если у Вас есть возможность нам помочь, то сообщаю дополнительно о узлах конструкции: тележка 4 wd, драйвер моторов L298N HEX (красная плата), ультразвуковой датчик HC-SR04, arduino UNO, хотелось бы продемонстрировать плавное движение (мне дети в ютубе показывали) тележки в лабиринте или просто прохождение лабиринта. Спасибо, что выслушали меня... /Владимир/
А вас разве берут с Ардуино на лего соревнования? Нас - нет, несмотря на то, что наш проект "Ардуино как лего" - сама плата и все датчики оформлены в лего-стандарте и позволяют использовать обычные лего детали для сборки конструкций. Тут есть проект с фотками, периодически обновляюсь. Да, и для 4wd надо 2 драйвера .. или у вас вариант 2/4wd (2 мотора на 4 колеса по одному на сторону)?
В ваш комплект крайне желательно добавить серводвигатель (пойдет и SG90) и посадить на него датчик расстояний. Из нашей лабиринтовой тележки остались только куски к моей библиотеке arhat.h .. осилите? :) Могу выложить код, который автономно и аппаратно крутит серву с датчиком и замеряет расстояния в массив, делая усреднения по 5 замеров. Можете поиграться, оно сильно облегчает работу с датчиком и расстояниями.
Код, да и библиотека заточена под Мегу, но может компилироваться и под УНО с соответствующими ограничениями (по аппаратной части: у Уны только 1 16-и битный таймер = только 2 аппаратных серводвигателя).
спасибо за ответ. Вот выдержка из положения наших соревнований:
Могу предложить такую раскладку пинов:
5,6 - ШИМ каналы 0-го таймера под сигналы EN ваших драйверов моторов для управления их скоростью. Занимаем один из входов компараторов и внешний счетный вход таймера 1, что в дальнейшем помешает подключить датчик цвета типа TSC3200 в режиме подсчета частоты, но зато можно будет использовать ICP1 для подсчета длины периода, что даже в среднем ускорит работу с ним. Использовать ШИМ от 2-го таймера, имхо хуже в перспективе, поскольку там сидит SPI интерфейс, а он может пригодиться, например для чтения с SD-карты, тех же программ или сохранения чего-либо "на лету" (дамп состояний для посметрной диагностики) .. так, "на будущее", раз у вас кружок;
9 - под управление серводвигателем радара. Он по сути один и остается, если резервировать 10..13 ноги под SPI. Для аппаратного управления серводвигателями удобнее пользовать 16-битный таймер, но можно приспособить и 2-й таймер тоже, с потерей точности. У меня под сервы использованы 16-битные, поэтому оставим 9 ногу;
Если резервировать остальное из таких же соображений совместимости датчиков и моторов, то под управляющие сигналы скорее всего придется отдать аналоговые входы 0..3. Остальное как-то жалко..
Ну и под радар можно использовать 4 и 7 пин, к примеру. Тут важно посмотреть какие датчики вы планируете далее использовать и в каком комплекте с моторами тележки для построения наиболее разнообразного количетсва вариантов .. кружок всеж-таки.
Вернусь, выложу скетч, точнее то что нашел от него с распиновкой под этот вариант.
Извиняюсь, с кодом будет задержка до выходных .. не добраться. :)
хорошо, не беспокойтесь
Вырезал из своего кода блок взаимодействия конечных автоматов для узв. датчика и серводвигателя и поменял номера пинов для раскладки выше. Ловите:
#include "arhat.h" #include "tsc.h" // ======== Маршевые моторы ======== // #define m0f 14 // Управление 4-я моторами "f" - вперед, "b" - назад #define m0b 15 #define m0s 5 // управление скоростью моторов через 8-бит ШИМ #define m1f 16 #define m1b 17 #define m1s 6 #define m0 0 // номера моторов в структуре pulses #define m1 1 #define MIN_SPEED 50 // наименьшая скорость моторов #define MAX_SPEED 255 // Наибольшая задаваемая скорость мотора #define MOTOR_WAIT_STOP 10 // *3 тактов МК. Задержка на рассасывание зарядов в драйвере. typedef struct{ TSC_Simple ctrl; // тут будем хранить текущее состояние КА uint8_t num; // номер мотора для распознавания "чей" получен указатель uint8_t speed; // заданная скорость разгона "до" [MIN_SPEED .. MAX_SPEED] } TSC_Motor; #define ptrMotor(ptr) ((TSC_Motor *)(ptr)) #define MAX_MOTORS 2 // Всего моторов TSC_Motor motor[MAX_MOTORS]; // тут будем хранить текущее состояние КА моторов [0..3] /** * Останов мотора */ void motorStop(uint8_t num) { switch(num){ case 0: pinOutLow(m0f); pinOutLow(m0b); break; case 1: pinOutLow(m1f); pinOutLow(m1b); break; } delayMicro8(MOTOR_WAIT_STOP); } /** * Устанавливает заданную скорость мотора */ void motorSpeed(uint8_t num) { switch(num){ case 0: pwmWrite(m0s, motor[num].speed); break; case 1: pwmWrite(m1s, motor[num].speed); break; } } /** * Включаем мотор "вперед" */ void motorForward(uint8_t num) { motorStop(num); switch(num){ case 0: pinOutHigh(m0f); break; case 1: pinOutHigh(m1f); break; } motorSpeed(num); } /** * Включаем мотор "назад" */ void motorBack(uint8_t num) { motorStop(num); switch(num){ case 0: pinOutHigh(m0b); break; case 1: pinOutHigh(m1b); break; } motorSpeed(num); } /** * Изменяет скорость мотора на заданный шаг */ void motorAddSpeed(uint8_t num, char step) { int speed = motor[num].speed; speed += step; if( speed < MIN_SPEED ){ speed = MIN_SPEED; } if( speed > MAX_SPEED ){ speed = MAX_SPEED; } motor[num].speed = (uint8_t)speed; motorSpeed(num); } // ======== Подключение ультразвукового датчика-измерителя расстояний HCSR-04 ======== // #define PCINT 2 // работаем с прерыванием уровня 2 #define MAX_PULSES2 1 // всего обслуживаем 5 прерываний: 4 от энкодеров и 1 узв. датчик. #define trigPin 4 // Датчик HCSR-04, сигнал trig подключен к этому пину Ардуино. #define echoPin 7 // .. его сигнал echo подключается сюда (Analog12) #define PULSE_ID 0 // .. его номер с массиве КА для измерения длительностей импульсов #include "hcsr04.h" // первое подключение библиотеки. tsc.h и pcint.h вызываются отсюда самостоятельно. // + создаст функцию startTrig4() -- для запуска датчика // + создает структуру pulses2[PULSE_ID] -- с данными КА датчика и результата работы #define pulses pulses2 #define sonicId PULSE_ID // ======================== Servo для HCSR-04 ======================== // #define pinSONIC 9 // серводвигатель датчика подключен как серво №1 enum ServoTypes { SG90 = 1, MG90, MG995, MG996 }; // типы серводвигателей: enum ServoDirs { TO_LEFT, TO_RIGHT }; // направления вращения если смотреть сверху на ось #define SG90_0 140 // SG90: минимальное значение ширины импульса будем считать это как "0 градусов" (вправо) #define SG90_1 3 // SG90: угол в градусах при повороте на 1 шаг #define SG90_180 620 // SG90: максимальное значение для моего двигателя. Будет "180 градусов" (влево) #define SG90_STEP1 8 // SG90: шаг с которым будем поворачивать двигатель (3 грд.) #define SG90_WAIT1 5 // SG90: =3*(120/60) + 2(запас) ТТХ сервы: за 0.12сек на 60грд! #define SONIC_STEPS 30 // всегда четное! шагов меньше на 1 чем положений! #define SONIC_STEP1 (SG90_180-SG90_0)/SONIC_STEPS // (620-140)/30=16 #define SONIC_WAIT1 (180/SONIC_STEPS)*(120/60)+2 // (180/16)*(120/60) + 2 = 14 мсек. на поворот #define MAX_TMPDIST 5 // количество замеров в одном направлении для усреднения показаний (минимально 3, поскольку 2 отбросятся!) #define MAX_ALLDIST (SONIC_STEPS+1) // ==31! Всего ячеек для измерений [0..180] #define DIST_NUM_FORWARD (SONIC_STEPS/2) // ==15! Номер ячейки, содержащей расстояние строго "вперед". typedef struct { TSC_Simple ctrl; // тут будем хранить текущее состояние КА серводвигателя uint16_t position; // текущее положение серводвигателя ServoDirs direction; // текущее направление куда крутимся вправо или влево? } TSC_Servo; #define ptrServo(ptr) ((TSC_Servo *)(ptr)) // преобразование в указатель на структуру КА для серводвигателей: TSC_Servo srvSonic; // Место для хранения состояний серводвигателя узв. датчика uint16_t tmpDistances[MAX_TMPDIST]; // Место хранения текущих замеров расстояний для повышения достоверности uint16_t curMeasure; // номер текущего замера расстояния uint16_t allDistances[MAX_ALLDIST]; // Все расстояния по направлениям uint16_t curDirection; // текущее направление измерения расстояния [0..MAX_ALLDIST-1] /** * Завершение текущей команды серводвигателя датчика: останов сервы и запуск измерений датчиком расстояний */ void servo_end(void *_ctrl) { // Serial.print(", servo end"); tsc_simple( &(ptrServo(_ctrl)->ctrl), 0, 1); // серве датчика: ничего не делать, проверять состояние каждую миллисекунду curMeasure = 0; // номер попытки замера дачтиком в этом направлении: "начни" pulses[sonicId].state = HCSR04_START; // запуск замеров датчиком } /** * Поворот серводвигателя на заданный угол: */ uint8_t srvServo_move(void *_tsc, uint16_t step) { uint16_t tmpPos = ptrServo(_tsc)->position + step; if( (tmpPos >= SG90_0) && (tmpPos <= SG90_180) ) { ptrServo(_tsc)->position = tmpPos; pwmWrite(pinSONIC, ptrServo(_tsc)->position); // поворачиваем серводвигатель на угол //Serial.print(", pos="); //Serial.print(ptrServo(_tsc)->position); return 1; } return 0; } /** * Поворот серводвигателя на 1 шаг вперед (влево, TO_LEFT) или назад (вправо, TO_RIGHT): */ void srvSONIC_next(void *_tsc) { //Serial.print(", servo next "); if( ptrServo(_tsc)->direction == TO_LEFT ) { if( !srvServo_move(_tsc, SG90_STEP1) ){ // Движение влево закончено, все замеры произведены. ptrServo(_tsc)->direction = TO_RIGHT; ptrServo(_tsc)->ctrl.command = 0; } } else if( ptrServo(_tsc)->direction == TO_RIGHT ) { if( !srvServo_move( _tsc, -SONIC_STEP1) ){ // Движение вправо закончено, все замеры произведены. ptrServo(_tsc)->direction = TO_LEFT; ptrServo(_tsc)->ctrl.command = 0; } } else return; // иначе ничего не делаем .. мало ли. tsc_simple( &(ptrServo(_tsc)->ctrl), servo_end, SONIC_WAIT1); // запуск обработки "серва повернулась" через SOINC_WAIT1 } /** * Вычисление среднего расстояния по нескольким измерениям. * Отбрасывает только 2 измерения "самое большое" и "самое маленькое", остальные усредняет * параметр - последнее измеренное расстояние - его уже некуда складывать! */ uint16_t avgDistance(uint16_t distance) { uint16_t maxD = 1, minD = 5000, sumD = 0; for(uint8_t i=0; i<MAX_TMPDIST; i++){ if( tmpDistances[i] > maxD ){ maxD = tmpDistances[i]; } if( tmpDistances[i] < minD ){ minD = tmpDistances[i]; } sumD += tmpDistances[i]; } if( distance > maxD ){ maxD = distance; } if( distance < minD ){ minD = distance; } sumD += distance; return (sumD - minD - maxD)/(MAX_TMPDIST-1); } /** * Вычисление среднего расстояния по нескольким измерениям. * Ничего не отбрасывает */ uint16_t avgDistanceAll() { uint16_t sumD = 0; for(uint8_t i=0; i<MAX_TMPDIST; i++){ sumD += tmpDistances[i]; } return sumD / MAX_TMPDIST; } /** * Сохраняем текущий замер в наборе временных расстояний или уже по направлению. * запускаем серводвигатель на поворот */ void saveDistance( void *_tsc, uint16_t distance ) { if( curMeasure < MAX_TMPDIST ) { tmpDistances[curMeasure] = distance; // если ещё повторяем попытки замера в направлении: curMeasure++; ptrPulse(_tsc)->state = HCSR04_START; //Serial.print(", mes"); //Serial.print(curMeasure, DEC); //Serial.print(" = "); //Serial.print(distance, DEC); } else { allDistances[curDirection] = avgDistance(distance); // Все замеры сделаны: усредняем и сохраняем по направлению. Последний (+1) замер отдаем как параметр! curMeasure = 0; // сброс попыток замера ptrPulse(_tsc)->state = 0; // Останавливаем замеры датчика. Запуск когда скажет "мозг" if( srvSonic.direction == TO_LEFT ) curDirection++; else curDirection--; tsc_simple( &srvSonic.ctrl, srvSONIC_next, 0); // Крутим серву датчика "как только, так сразу" //Serial.print(", Distance="); //Serial.print(allDistances[curDirection], DEC); } } /** * Внутренние состояния датчика и его действия: */ void doHCSR04(void *_tsc) { //Serial.print(", hcsr04 ="); //Serial.print(ptrPulse(_tsc)->state, DEC); switch( ptrPulse(_tsc)->state ) { case HCSR04_START: //Serial.print(", start"); tsc_simple((TSC_Simple *)_tsc, startTrig4, 1); // запуск замера датчиком: ptrPulse(_tsc)->state = 0; // .. сброс состояния, чтобы ничего в switch не делать... нечего. Просто ждем. break; case PULSE_OK: // замер произведен, данные - действительны: saveDistance(_tsc, getDistance(ptrPulse(_tsc))); //Serial.print(", ok"); break; case PULSE_TIMER: // или сработал КА по таймауту. saveDistance(_tsc, HCSR04_MAX_DISTANCE); // .. слишком далеко, заменяем на предельно измеряемое расстояние в мм. //Serial.print(", timer"); break; case PULSE_ERROR: ptrPulse(_tsc)->state = HCSR04_START; // ошибка. Перезапуск попытки замера. //Serial.print(", error"); break; // default: //Serial.print(", default"); } } // *********** начальные установки: **************** // /** * задержка-моргалка 13 ногой. Диагностика перезагрузки МК по питанию * если перезагрузка, то робот встает "колом" на 7.5сек. - ЗАМЕТНО!! */ void doBlink() { for(uint8_t i=5; i>0; i--){ pinOutHigh(pinLed); delay(300); pinOutLow(pinLed); delay(200); } delay(2000); } void setup() { pinModeOut(trigPin); // нога trig датчика HCSR-04 "на выход" pinModeIn(echoPin); // нога echo датчика на вход pulses[sonicId].state = 0; // выключаем датчик до завершения поворота серводвигателя tsc_simple( &(pulses[sonicId].ctrl) // запускаем КА датчика , 0 // .. ничего не делать, , 1 // .. но проверять каждую миллисекунду завершение замера по прерываниям ); srvSonic.position = SG90_0; // начинать будем с положения "0" srvSonic.direction = TO_LEFT; // "0" - справа!! pwmSetServo(pinSONIC); // включаем режим "аппаратное серво" для таймера этого пина pinModeOut(pinSONIC); // и открываем её на "вывод" pwmWrite(pinSONIC, srvSonic.position); // устанавливаем серву в правое положение tsc_simple( &(srvSonic.ctrl) // запускаем КА серводвигателя , servo_end // .. доехал в "0" , SONIC_WAIT1*SONIC_STEPS // .. мало ли куда стояла серва до включения.. ); curMeasure = 0; // настройка номера текущего замера curDirection = 0; // .. по направлению (самое правое!) pinModeOut(m0f); // управление моторами тоже ставим "на выход" pinModeOut(m0b); pinModeOut(m1f); pinModeOut(m1b); pwmSet(m0s); // разрешаем управление скоростью моторов pwmSet(m1s); for(uint8_t i=0; i<MAX_MOTORS; i++){ motor[i].num = i; // сохраняем номер мотора в его КА motor[i].speed = 0; // скорость = 0 motorStop(i); // останавливаем мотор } // Serial.begin( 9600 ); doBlink(); } // *********** повторяем это: ************************// void loop() { //Serial.print("loop "); //Serial.print(getOvfCount(), DEC); tsc_run( &srvSonic.ctrl ); // шаг КА серводвигателя датчика расстояний tsc_run( &pulses[sonicId].ctrl ); // шаг КА датчика: таймаут, запуск, или ничего не делает. doHCSR04(&pulses[sonicId]); // отдельно исполняем логику автомата "узв. датчик". //Serial.print(", run:"); //Serial.print((uint16_t)(brain.ctrl.command), DEC); //Serial.print(":"); //Serial.print(tsc_getTime(), DEC); //Serial.print(","); //Serial.print(brain.ctrl.started_at, DEC); //Serial.print(","); //Serial.print(brain.ctrl.timeout, DEC); //Serial.println(""); }Комментарии не подправил .. остались от моего варианта с 4 моторами и энкодерами.
Здравствуйте, после ознакомления с Вашим кодом получил бесконечную зависть к автору и осознание своей полной "безграмотности"...Вы молодец...я много видел кодов для такой машинки, но этот...(почти как в фильме "Классик") и если он еще у нас заработает...
с сервой и радаром, как с описанием, так и с подключением более-менее понятно
Пожалуйста, если можно, объясните строки с моторами (маршевые моторы - 6 строк) - мне не совсем понятно как связаны ардуино UNO с драйвером моторов:
драйвер ардуино UNO
ENA-----------? пин
IN1-
IN2-
IN3-
IN4-
ENB-
Пины моторов прописаны в строках с 6 по 15:
IN1 = m0f (motor0, forward) = 14 пин, он же Analog0 UNO
IN2 = m0b (motor0, back) = 15 пин, он же Analog1 UNO
ENA = m0s (motor0, speed) = 5 пин, он же PWM5
IN3 = m1f = 16 пин (Analog2), IN4 = m1b = 17 пин, он же Analog3, ENB = m1s = 6 пин, он же PWM6
В самом скетче нет управления моторами, только взаимодействие между серводвигателем и узв. датчиком в виде конечных автоматов:
В setup() серводвигатель запускается в крайнее левое положение (#321..324) и выставляется ему макс. пауза для "доезда".
По завершению движения сервы его автомат (#352) вызовет servoEnd(), которая отключает себя и разрешает КА датчика начать замер (#150).
Логика автомата датчика ( обрабатывается отдельно, в #354 каждый вызов loop) тупо проставит запуск отправки импульса для начала замера (функция startTrig4, #270), который излучает импульс и настраивает прерывание на подсчет длительности отклика. Само измерение производится в процедуре обработки прерывания и тут не видно (pcint.h, pcint.c библиотеки, там же есть подсчет энкодеров таким же способом, тут не используются). По завершению замера из прерывания будет изменен статус автомата датчика, что и будет обнаружено в его логике работы dohcsr04() строки #263..#288. В случае ошибки замера - производится перезапуск датчика через смену состояния автомата. В случае удачного замера - вызывается saveDistance() в #274, которая ведет учет замеров, занимается их усреднением и складывает результат в текущую ячейку массива расстояний по направлениям allDistances[] и переводит текущее направление, смотря куда вертится серводвигатель #251,252.
По завершению замеров в текущем направлении, усреднении результата и складыванию его в текущую ячейку массива расстояний, датчик запускает серводвигатель на поворот #254 и остается в режиме ожидания. Автомат серводвигателя (#352) вызовет srvSonic_next(), которая пытается установить новое направление для серводвигателя через вызов srvServo_move(). Последняя есть не что иное, как проверка выхода за допустимые значения и собственно установка значения PWM на ноге сервы. Функция srcSonic_next() в общем-то тупо переключает направления и указывает что следующим шагом автомата серводвигателя надо вызвать srv_end() - "серва доехала", которая останавливает серву и запускает замеры .. и так по кругу. :)
Чтобы далее управлять моторами можно в loop дописать код, анализирующий расстояния и соответственно принимающий решения какой мотор куда включать и куда смотреть далее. Если не вмешиваться в работу пары серва-датчик, то они так и будут замерять расстояния, вращяясь туда-сюда обратно.
Наш код я отсюда повырезал, ибо он во-первых, писан ребенком по большей части, а во вторых жутко искалечен последними переделками и недоработками и, разбираться что в нем верно а что не так - я не стал. У меня он даже не компилировался. Что-то там последний раз он недоделал похоже. Ну и в третьих, он содержал в себе достаточно много ерунды в виде "таблицы Брадиса" на заданные основные углы пересчетов направлений при пересчете в декартовы координаты "на лету" и поиску какой угол лабиринта мы сейчас объезжаем .. в общем: не полезно и не интересно ни разу и точно не поможет, а только больше запутает.