Перейти к содержимому


Фотография
- - - - -

Мой первый самостоятельный исследователь окружающей среды!

Робот на базе dvrobot компоне

  • Авторизуйтесь для ответа в теме
Сообщений в теме: 6

#1 akatla

akatla

    Новичок

  • Пользователи
  • Pip
  • 3 сообщений
  • ГородХабаровск

Отправлено 13 Ноябрь 2014 - 02:47

Всем доброго времени суток!

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

 

GigRobot3.jpg

 

Рис 1.

 

 

Итак идем дальше. Достаточно долго я размышлял над платформой как и что именно выполнить, может еще до конца не понимая чего же я вообще хочу добиться. И тут я наткнулся на одну интересную публикацию в которой описывалось как из радиоуправляемой игрушки сделать робота который обходит препятствия. Конструктивно, все базировалось на сервоприводе с закрепленным датчиком расстояния, именно по этому мне и пришла в голову мысль. совместить две платформы имеющиеся у DvRobot одновременно вот эту:

 

 

Рис 2.

 

и эту платформу:

 

 

Рис 3.

 

у первой из них, я использовал, только верхнюю часть, так как в ней уже выполнено крепление для серво привода именно в передней части. У второй платформы я верхнюю часть отрезал примерно по середине. Таким  образом IR датчик расстояния был как раз закреплен в нужном месте и основной батарейный отсек встал ровно на втором этаже моей конструкции, саму "Дуню" и макетную плату я установил на третьей "палубе"! :) В результате получилось вот это:

 

 

Рис 4.

 

Далее я начал с тестирования и подключения двигателей для получения вращения в верном напрвлении и сервомотора. Как оказалось, в моей модификации "Дуни" Leonardo для библиотеки "Servo" он закреплен за 9-ым пином GPIO с PWM. В общем вот в этой корзинке я все это и собрал и тестировал:

 

 

Рис 5.

 

В целом все запустилось и работало без сбоев и серводвигатель поворачивался ровно на те градусы которые я его и просил!

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

 

 

Рис 6.

 

и далее:

 

 

Рис 7.

 

в результате получилась основа имеющая вот такой вид:

 

http://ineedtowing.c...ges/TeleGA5.jpg

Рис 8.

 

Здесь я уже все подготовил для того что бы опробовать как все это будет двигаться и как запустить на тестирование Encoders. Платы управления двигателями были готовы к работе и я сразу использовал ШИМ  (PWM) подключение что в данном случае было достаточно целесообразно. Сразу приведу мою раскладку подключения к "Дуне" плат управления двигателями. Вы можете выбрать свой порядок но для меня этот вариант стал в конечном итоге достаточно оптимальным:

 

Здесь полное описание L298N two motors driver:

http://tronixlabs.co...2a-and-arduino/

 

http://ineedtowing.c...rs1Drivers1.jpg

Рис 9.

 

Вторая раскладка прописана для второй карты управления двигателем. так же в моем случае я использовал схему с раздельным питанием силовой части и самого контроллера. Не забудьте соединить GND (землю) обоих модулей питания вместе иначе схема не "заведется"!  ;-) Итак двигатели готовы и я приступил к построению простых алгоритмов движения с целью выявить особенности конструкции " тележки". особенно мне понравилось пускать левые двигатели вперед а правые назад получался некий "танковый" разворот что выглядело достаточно забавно. В моем случае я включил правую сторону к одному контроллеру двигателей, а левую к другому. что позволяло выполнять повороты в случае встречного вращения моторов, обоих или только двух. В вашем случае можете поэкспериментировать так как это нужно вам.

 

Далее я подключил Encoders датчики вот в такой раскладке (советую и вам сделать тоже самое):

 

http://ineedtowing.c...s/Encoders1.jpg

Рис 10.

 

и датчики касания - закрепленные впереди:

 

http://ineedtowing.c...ashSensors1.jpg

Рис 11.

 

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

 

Далее я подключил сервопривод вот в такой раскладке:

 

http://ineedtowing.c.../ServoPins1.jpg

Рис 12.

 

и собственно сами "Глаза" IR датчик Sharp вот в такой раскладке:

 

http://ineedtowing.c...SensorConn1.jpg

Рис 13.

 

вот собственно так были проведены все основные соединения с "Дуней" и в данном конкретном случае все было готово к написанию комплексного скетча управления всей платформой. Которая к тому моменту выглядела примерно вот так:

 

http://ineedtowing.c.../StartWork4.jpg

Рис 14.

 

Что ж сразу оговорюсь, что это описание рассчитано на подготовленного пользователя который уже умеет что то делать руками и имеет элементарные понятия в физике, электронике и English language и языке программирования ANSI C! Я могу пойти как говориться с нуля. Но я думаю будет проще если Вы просто изложите что конкретно вам не понятно и мы уже в дальнейшей дискуссии согласно Ваших конкретных навыков постараемся разобраться и помочь решить проблему при повторении или частичном использовании моей конструкции. Продолжим чуть позже . . .

 

Думаю, что самое время продолжить дальше. Надеюсь вам более менее понятно до настоящего момента как соеденить все части в единое целое. Если да, то займемся Encoders. Подключения самих датчиков, я показал вам на Рис 10 ранее при описании подключений. Encoders это две планки с двумя парами датчиков которые закреплены над каждой парой двигателей на противоположные калесам полуоси одеты "крыльчатки" колесики с радиально расположенными окошками. Всего их там 20ть. При вращении двигателей датчики начинают выдавать импульсы на подключенные GPIO, в соотвествии с которыми программными методами можно заставить двигатели вращаться например на определенный угол или выполнить заданное количество оборотов. Для того что бы со всем этим разобраться я написал, простой тэстовый скетч и протестировал сколько приходит импульсов на порт GPIO если колесо провернуть например на 360 градусов, тоесть совершить полный оборот. Еще хочу заострить ваше внимание на одном очень важном моменте, что при выборе портов для Encoders я руководствовался не только удобством их использования для совместимости с массивом и упрощением кода программы  - но и тем фактом что для моей Leonardo (у вас может быть по другому) по спецификации именно GPIO порты 0, 1, 2, 3 поддерживают аппаратное прерывание. Описание можно найти вот тут, прочтите его внимательно:
 

http://arduino.cc/en...AttachInterrupt

 

Вот листинг самой программы:
 

// Тестирование датчиков Encoders.

// Задаем номера портов GPIO на контроллере Arduino
// в нашем случае используем 0, 1, 2, 3 порты GPIO.
#define FLEFT  0 
#define FRIGHT 1

#define RLEFT  2
#define RRIGHT 3

// Переменная для организации временного шага считывания
// с датчиков Encoders.
static unsigned long timer = 0;

// Массив для хранения тиков поступающих с датчиков Encoders.
long coder[4] = { 
                  0,
                  0,
                  0,
                  0                  
                };

// Массив для хранения промежуточных данных поступающих с датчиков Encoders.
int lastSpeed[4] = { 
                     0,
                     0,
                     0,
                     0
                   };  

// Предустановочная функция контроллера.
void setup()
{
  // Настраиваем скорость COM порта для обмена данными через Terminal Window.
  Serial.begin(9600);                                   // init the Serial port to print the data
  
  // "Прицепляем" функции обработчики прерываний поступающих с портов GPIO от датчиков Encoders.
  // Всего четыре функции для четырех двигателей.
  attachInterrupt(FLEFT,  LwheelSpeed, CHANGE);         // init the interrupt mode for the digital pin 0
  attachInterrupt(FRIGHT, RwheelSpeed, CHANGE);         // init the interrupt mode for the digital pin 1
  attachInterrupt(RLEFT,  FLwheelSpeed, CHANGE);        // init the interrupt mode for the digital pin 2
  attachInterrupt(RRIGHT, FRwheelSpeed, CHANGE);        // init the interrupt mode for the digital pin 3
  
}

// Основная функция контроллера.
void loop()
{
  
  if(millis() - timer > 200)
  {                   
    // Выводим данные с датчиков на  Serial Monitor.      
    Serial.print(coder[FLEFT]);
    Serial.print(" [Front Left Wheel ] ");
    Serial.print(coder[FRIGHT]);
    Serial.print(" [Front Right Wheel] ");

    Serial.print(coder[RLEFT]);
    Serial.print(" [Rear Left Wheel ] ");
    Serial.print(coder[RRIGHT]);
    Serial.println(" [Rear Right Wheel] ");

    lastSpeed[FLEFT]  = coder[FLEFT]; 
    lastSpeed[FRIGHT] = coder[FRIGHT];
    lastSpeed[RLEFT]  = coder[RLEFT]; 
    lastSpeed[RRIGHT] = coder[RRIGHT];

    coder[FLEFT]  = 0;                
    coder[FRIGHT] = 0;
    coder[RLEFT]  = 0;                
    coder[RRIGHT] = 0;
        
    timer = millis();
     
  }
  
}

// Функции обработчики событий поступающих с портов GPIO Arduino.
// В данном случае "тики" от вращения двигателей робота.
void LwheelSpeed()
{
  coder[FLEFT] ++;  // count the front left wheel encoder interrupts
}

void RwheelSpeed()
{
  coder[FRIGHT] ++; // count the front right wheel encoder interrupts
}

void FLwheelSpeed()
{
  coder[RLEFT] ++;  //count the rear left wheel encoder interrupts
}

void FRwheelSpeed()
{
  coder[RRIGHT] ++; //count the rear right wheel encoder interrupts
}

Для того что бы проверить исправность датчиков и убедиться что все порты GPIO подключены и работают верно, запустите среду разработчика "Дуни", загрузите мой скетч и не отключая "Дуню" от порта компьютера запустите Serial Monitor (заначек линзы с плюсом внутри в правом верхнем углу окна) как показано на рисунке:

 

http://ineedtowing.co/images/Otll1.jpg

Рис 15.

После этого вы увидите примерно вот такую картинку:
 

http://ineedtowing.co/images/Otll2.jpg

Рис 16.

В моем случае я вращал просто руками правое переднее колесо робота и получал случайные числа от датчика. Для того что бы точно посчитать количество "тиков" на один оборот (360 градусов), нужно закомментировать вот этот кусочек кода скетча:
 

.
.
/*
    coder[FLEFT]  = 0;                
    coder[FRIGHT] = 0;
    coder[RLEFT]  = 0;                
    coder[RRIGHT] = 0;
*/
.
.
.

В этом случае массив будет сохранять значения при любом вращении колеса. Запустите скетч и поставте на колесе маркером метку. Проверните колесо от метки до положения пока метка не вернется на место откуда она начала движение. Посмотрите какое число появиться в заданной ячейке массива. В моем случае количество тиков после совершения полного оборота колеса робота соответствовало 36. То есть один "тик" (импульс) на 10 градусов поворота колеса. В данном случае, можно легко произвести расчет к примеру какое расстояние пройдет робот при совершении одного оборота колеса, что позволит вам например точно направлять его на заданное расстояние если у вас возникнет такая необходимость. Итак всем известно что длинна окружности равна: L = 2 * на r (в данном случае радиус колеса) * на число "пи". Так вот в моем случае это будет L = 2 * 32.5 * 3.14 что приблизительно равно 204 мм. То-есть мой робот при полном обороте колес проходит 20.4 см. Поэкспериментируйте с Encoders - уверен это будет очень интересно и доставит вам массу удовольствия и неожиданных открытий. К примеру, заставте его проехать ровно метр, замечу что при движении у робота есть инерционная составляющая так как физику никто не отменял, в определенный момент при попытке его остановить, он какое то время будет "докатываться", подумайте как этого избежать! Рассмотрим наш пример с движением на расстояние. 1м это 1000 см. Делим 1000 см на 204 мм, это приблизительно 4,9 умножаем на 36 тиков на оборот, получаем что для того что бы робот прошел примерно 1м нужно подать команду stop на двигатели после 177 тиков Encoders. Или если один "тик" соответствует 10 градусам поворота, то колесо должно "намотать" 1770 градусов! Проверте меня если, если я не прав. Но думаю в каждом конкретном случае, все будет согласно вашей конструкции робота. Думаю, что для начала с Encoders мы разобрались, жду ваших вопросов и замечаний . . .

 

Следующий этап, IR (инфракрасный) датчик расстояния. Скажу сразу, что я много по своей нынешней специфике, работал с LEGO NXT. Они используют в большей части ультразвуковые датчики расстояния, но когда я увидел SHARP IR сенсор мне просто стало интересно как с ним заставить робота что то делать. В том проекте который я упомянул вначале своего повествования, тележка была выполнена с применением именно этого датчика, да и на dvrobot были все крепления для этого датчика с том числе и на сервопривод, были в наличии. Работает он как оказалось достаточно просто и его установка и запуск не потребовали особых усилий. В датчике уже встроен контроллер который меняет напряжение на его сигнальном пине от 0 до примерно 3.0 вольт в соответствии расстоянию до объекта. Но график изменения несколько нелинеен. Вот как он выглядит:

 

http://ineedtowing.co/images/Otll4.jpg

Рис 17.

Сначала меня несколько смутила эта кривая и я решил поэкспериментировать сам что бы увидеть как он ведет себя в реальной жизни. Порывшись пару часов в сети я нашел вот такой скетч для его проверки:
 

// Пин для датчика соотвествует моей схеме.
// analog pin for reading the IR sensor
int IRpin = 0;                                    

void setup() {
  // start the serial port
  Serial.begin(9600);                             
}

// Функция переводит значение в вольтах поступающих с датчика
// в расстояние до препятствия в сантиметрах. Что упростило
// мой код в дальнейшем.
float CheckIPSensor()
{
  float volts = analogRead(IRpin) * 0.0048828125;    // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  float distance = 65 * pow(volts, - 1.10);          // worked out from graph 65 = theretical distance / (1/Volts)    
  return (distance);
}
 
void loop() {
  
  Serial.println(CheckIPSensor());                   // print the distance
  delay(100);                                        // arbitary wait time.
  
}

В результате, я выяснил один интересный момент в его работе. Он схватывает расстояние ровно до диапазона 21-22 см, затем начинает подвирать в части того что дальнейшее приближение вызывает увеличение показаний, а не их уменьшение. Что собственно и соответствует его вольт-метровой характеристике. Вот картинка которую дает скетч с запущенным Serial Monitor:

 

http://ineedtowing.co/images/Otll5.jpg

Рис 18.

После запуска скетча поставте препятствие перед датчиком и проследите за его показаниями. В моем случае до 21 см, все измеряется корректно. Если приблизить ближе, показания становятся не верными. И еще один недостаток, я думаю возможен в применении IR дальномера - прямая засветка солнцем или источником стороннего IR или лазерного излучения. Честно, я не пробовал, но такая вероятность есть, думаю проверить по возможности. Если кто имел опыт в этом поделитесь, буду рад узнать как он реагирует на это. В остальном же нареканий датчик не вызывал и в комнатных условиях работает достаточно точно и четко. IR сенсор я установил на сервопривод (Рис 12 см. выше) который постоянно вращает датчик на угол от 0 до 180 градусов. Таким образом робот в процессе движения как бы постоянно осматривается получая информацию о препятствиях перед ним в непрерывном режиме. Это обеспечивается, двумя циклами for в основном loop цикле робота. Первый цикл прокручивает датчик с 0 до 180, а второй от 180 до 0. Примерный шаг датчика 8 градусов. Количество итераций в цикле измерения расстояний 23 то есть 12-я итерация производит замер расстояния прямо перед роботом. Все остальные уже под определенным углом к движению. Массив с расстояниями (имеет имя irvalarr в скетче) подлежащими реакции на разворот от препятствия находится в начале скетча в секции определении глобальных переменных. Вы можете подбирать расстояния по вашему усмотрению в соответствии с конструкцией вашего робота и применяемого вами датчика расстояния. Функция checkDist является базовой и осуществляет расчет времени и стороны поворота робота для ухода от препятствия. Алгоритм этой функции я не буду описывать, пусть это будет вашим домашним заданием, но скажу только что она базируется на булевой функции "импликация":
 

https://ru.wikipedia...wiki/Импликация

 

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

 

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

/* 
    This function reads the IR sensor and has the car respond when the range to 
    sensor pin reads is less than the value of irvallarr[] that corresponds 
    with the servo angle. 
*/
void checkDist(int servtime, int PWr)
{
    // Switch crash sensor Right.
    if(digitalRead(CSl) == LOW)
    {
        if(TimeEncode)
          TurnL(PWr);
        else
          aEncoderLeft(); 
    }

    // Switch crash sensor Left.
    if(digitalRead(CSr) == LOW)
    {
       if(TimeEncode)
          TurnR(PWr);
       else
          aEncoderRight();
    }
 .
 .
 .
 .

Безусловно такое решение не является оптимальным - но тем не это компенсирует особенности конструкции робота и применяемого  контроллера, в следствии того что угол осмотра IR сенсора 180 градусов переднего сектора робота. Вы можете сами придумать  какой либо другой способ внеся в конструкцию свою доработку. На этом основная часть конструкции тележки нашего бота вам  уже должна быть ясна. Теперь давайте немного остановимся на ходовой части и то как реализуется управление движением. В моем  боте я применил две карты управления двигателями:

 

http://dvrobot.ru/237/221.html

 

которые в свою очередь позволяют обслуживать два мотора осуществляя переключение  направления движения путем смены логических уровней на управляющих пинах и реализуют такую функциональность как ШИМ (PWM)  так называемую "широтно импульсную модуляцию" или PWM (Pulse Width Modulation), а именно плавное изменение скорости  вращения моторов. Я думаю, в этой тематике у многих из вас весьма запутанные познания, давайте попробуем разобраться  что же такое ШИМ (PWM). К примеру если возьмете мотор своей платформы и подключите к нему батарею, он начнет вращение с  той скоростью на которую способен от вашей батареи, а именно это зависит от того сколько вольт вы на него подаете - 9,  12 или более, какова емкость вашей батареи, аккумулятора и какова степень их разряда или срока службы и какой максимальный  ток они способны отдавать. Я не буду сейчас останавливаться на таких понятиях как полярность, ток, напряжение, сопротивление,  закон Ома и т.д. Надеюсь вы имеете об этом некоторые понятия. Так вот двигатель вращается и развивает некоторую мощность.  А вот, что делать если количество оборотов двигателя нужно менять динамически в процессе движения робота? Что делать тогда?  Давайте посмотрим на следующий рисунок - принципиальную схему:

 

http://ineedtowing.c...ages/PWMRez.gif

Рис 19.
 
Здесь, двигатель включен к источнику питания через переменный резистор. Меняя положение движка резистора можно менять, скорость  вращения двигателя. Но, здесь есть небольшое но. Потребляемый ток у двигателя довольно большой и переменный резистор, должен быть  большой мощности и тем более двигать его с помощью программного кода контроллера не выйдет. Так что будем делать? Верно применим  PWM controller. Для начала давайте определимся с основополагающим аспектом PWM (ШИМ) - базируется он на изменении так называемой "скважности  импульса" подаваемого на переключающий элемент (обычно это транзистор средней или большой мощности который позволяет коммутировать,  управлять большими токами) Посмотрите на вот этот рисунок:
 

http://ineedtowing.co/images/PWM3.gif

Рис 20.
 
Здесь T - это период следования импульса, "скважностью" (наверное мало кто из моих читателей, слышал это слово, но тем не менее) в этом случае называется  отношение длительности импульса, к его периоду следования. Период следования импульса T может быть к  приеру 1с, одни импульс -  что будет равно 1 герц. Но длительность импульса при этом может быть 1 мКс. Теперь посчитайте скважность - верно 1000 (еще ее можно измерять  в процентах в данном случае это будет 0,001%), то есть в нашем случае мы имеем 0 вольт в течении 0,999 сотых секунды и только 1 мКс мы будем иметь 5 вольт.  Что на рисунке прописано как Narrow pulse и Low voltage. Если мы будем подавать на коммутирующий элемент - в нашем случае транзистор:

 

http://ineedtowing.c...ages/PWM4_4.gif

Рис 21.

импульсы такой скажности, двигатели не будут вращаться, в следствии того что, транзистор будет открыт и насыщен только в течении 0,001% времени отпущенного ему за 1с периода колебания. Если мы будем плавно увеличивать длительность импульса до уровня 90,999% процента, то двигатель начнет вращаться с максимальной мощностью. Как правило PWM (ШИМ) использует от 245 герц до примерно 25 кГц период следования импульсов управления для получения максимальной эффективности и КПД. К стати для полноты картины, замечу что транзистор в данном случае используется именно как ключ - то есть либо закрыт, либо открыт и насыщен. А, импульсы прямоугольной формы - подаваемые на его электрод под именем БАЗА, называются меандр! И еще, если мы взглянем на электрод под названием КОЛЛЕКТОР в момент коммутации, а именно при подачи импульсов с изменяемой скважностью, то там будут импульсы похожие на ступеньки лестницы:

 

http://ineedtowing.co/images/PWM2.png

Рис 22.

их высота в вольтах будет плавно меняться,  в следствии того что пара транзистор двигатель и диод подавления, обратного тока образуют так называемую - индуктивно емкостную цепь имеющую свой гистерезис или индуктивно резистивную составляющую. В результате входной сигнал прямоугольной формы преобразуется, транзистором, двигателем, диодом и проводами подключения платы управления в "лесенку". Пока импульс активен на БАЗЕ транзистора, он производит заряд всей этой цепи, как только он падает до нуля, начинается разряд всей этой цепи и т.д. В результате получается "лесенка" - высота которой в вольтах и определяет мощность подаваемую на двигатель. Я попытался объяснить это как можно проще - надеюсь вам было понятно, но на самом деле за PWM (ШИМ) стоят гораздо более емкие понятия математические формулы, не буду вас ими удосуживать, если кому либо интересно - пишите я расскажу об этом более детально. Надеюсь до этого момента вы все же смогли понять как с помощью коммутирующего элемента и изменения скважности импульса можно плавно менять скорость вращения двигателя вашего робота.

И наконец в заключительной части моего описания, я приведу полный код на C для моего робота - "Маленький исследователь":

#include <Servo.h>

/* *********************************************************

Robot Explorer! @ 2014.

1) 36 tics for 360 degree.
2) Version 0.0.3.3 (16.11.2014)

********************************************************* */

#define FLEFT  0
#define FRIGHT 1

#define RLEFT  2
#define RRIGHT 3

Servo servoTest;

long coder[4] = { 
                  0,
                  0,
                  0,
                  0                  
                };

int lastSpeed[4] = { 
                     0, 
                     0, 
                     0, 
                     0 
                   };

// First Controller of Motors.
int ENAR = 13;
int ENBR = 11;
int IN1  = 12;
int IN2  = 8;

// Second Controller of Motors.
int ENAL = 10;
int ENBL = 6;
int IN5  = 7;
int IN6  = 4;

float irVal = 0.0;
int servPin = 9;          // Servo is pin 9 !!! Note that in Arduino 0016 and earlier, the Servo library supports only servos on only two pins: 9 and 10. 
int PWRStart = 100;
bool TimeEncode = true;   // Moving use encoders or time stamp.

int CSl = 5;  // Crash sensor left.
int CSr = A1; // Crash sensor right.

// This array is a series of values collected that correspond to distance readings at different servo angles.
float irvalarr[23] = { 21.0, 22.0, 24.0, 25.0, 27.0, 29.0, 31.0, 32.0, 34.0, 36.0, 38.0,  40.0,  38.0, 36.0, 34.0, 32.0, 31.0, 29.0, 27.0, 25.0, 24.0, 22.0, 21.0 };

void DropCoders() 
{
   
   coder[FLEFT]  = 0;                
   coder[FRIGHT] = 0;
   coder[RLEFT]  = 0;                
   coder[RRIGHT] = 0;     
   
   lastSpeed[0] = 0;
   lastSpeed[1] = 0;
   lastSpeed[2] = 0; 
   lastSpeed[3] = 0;
   
}

// Stop 1 sec.
void pitStop()
{
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,LOW);
  digitalWrite(A2, LOW);
  digitalWrite(A3, LOW);  
   analogWrite(ENAR, 0);
   analogWrite(ENBR, 0);

  digitalWrite(IN5,LOW);
  digitalWrite(IN6,LOW);
  digitalWrite(A4, LOW);
  digitalWrite(A5, LOW);
   analogWrite(ENAL, 0);
   analogWrite(ENBL, 0);
}

void GoForvard100(int Pwr)
{
  // Go forward.
  int p = map(Pwr, 0, 100, 0, 255);  

  digitalWrite(IN5,LOW);
  digitalWrite(IN6,HIGH);
  digitalWrite(A4, LOW);
  digitalWrite(A5, HIGH);
   analogWrite(ENAL, p);
   analogWrite(ENBL, p);

  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(A2, HIGH);
  digitalWrite(A3, LOW);
   analogWrite(ENAR, p);
   analogWrite(ENBR, p);  
}

void GoBack100(int Pwr)
{
  // Go back.  
  int p = map(Pwr, 0, 100, 0, 255);  
  
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(A2, LOW);
  digitalWrite(A3, HIGH);
   analogWrite(ENAR, p);
   analogWrite(ENBR, p);
  
  digitalWrite(IN5,HIGH);
  digitalWrite(IN6,LOW);
  digitalWrite(A4, HIGH);
  digitalWrite(A5, LOW);  
   analogWrite(ENAL, p);
   analogWrite(ENBL, p);  
}

void MakeALeft100(int Pwr)
{
  // Make a left.
  int p = map(Pwr, 0, 100, 0, 255);  

  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(A2, HIGH);
  digitalWrite(A3, LOW);
   analogWrite(ENAR, p);
   analogWrite(ENBR, p);

  digitalWrite(IN5,HIGH);
  digitalWrite(IN6,LOW);
  digitalWrite(A4, HIGH);
  digitalWrite(A5, LOW);
   analogWrite(ENAL, p);
   analogWrite(ENBL, p);
}

void MakeARight100(int Pwr)
{
  // Make a right.
  int p = map(Pwr, 0, 100, 0, 255);  
  
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(A2, LOW);
  digitalWrite(A3, HIGH);
   analogWrite(ENAR, p);
   analogWrite(ENBR, p);
 
  digitalWrite(IN5,LOW);
  digitalWrite(IN6,HIGH);
  digitalWrite(A4, LOW);
  digitalWrite(A5, HIGH);  
   analogWrite(ENAL, p);
   analogWrite(ENBL, p);
}

void TurnL(int pwr)
{
  GoBack100(pwr);
  delay(750);
  pitStop();
  delay(100);
  MakeALeft100(pwr);
  delay(900);
}

void TurnR(int pwr)
{
  GoBack100(pwr);
  delay(750);
  pitStop();
  delay(100);
  MakeARight100(pwr);
  delay(900);
}

void turnAroundL(int pwr)
{
  GoBack100(pwr);
  delay(500);
  pitStop();
  delay(100);
  MakeALeft100(pwr);
  delay(1200);
}

void turnAroundR(int pwr)
{
  GoBack100(pwr);
  delay(500);
  pitStop();
  delay(100);
  MakeARight100(pwr);
  delay(1200);
}

// ******************** Make a moving using encoders *********************************************

void GoForvardL(int Pwr)
{
  int p = map(Pwr, 0, 100, 0, 255);  

  digitalWrite(IN5,LOW);
  digitalWrite(IN6,HIGH);
  digitalWrite(A4, LOW);
  digitalWrite(A5, HIGH);
   analogWrite(ENAL, p);
   analogWrite(ENBL, p);
}

void GoForvardR(int Pwr)
{
  int p = map(Pwr, 0, 100, 0, 255);  

  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(A2, HIGH);
  digitalWrite(A3, LOW);
   analogWrite(ENAR, p);
   analogWrite(ENBR, p);  
}

void GoBackR(int Pwr)
{ 
  int p = map(Pwr, 0, 100, 0, 255);  
  
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(A2, LOW);
  digitalWrite(A3, HIGH);
   analogWrite(ENAR, p);
   analogWrite(ENBR, p);  
}

void GoBackL(int Pwr)
{
  int p = map(Pwr, 0, 100, 0, 255);    
  
  digitalWrite(IN5,HIGH);
  digitalWrite(IN6,LOW);
  digitalWrite(A4, HIGH);
  digitalWrite(A5, LOW);  
   analogWrite(ENAL, p);
   analogWrite(ENBL, p);  
}

void nMotorEncoderTargetLS(int pWR, int trg)
{    
   if(coder[FLEFT] < trg && coder[RLEFT] < trg)
   {
      while(coder[FLEFT] < trg && coder[RLEFT] < trg)
      {
         if(pWR > 0)
         {
           GoForvardL(pWR);
           GoForvardR(pWR); // Slave
         }
         else
         {
           GoBackL(pWR * -1);
           GoBackR(pWR * -1); // Slave
         }       
      }
    
      if(pWR > 0)
      {
        GoBackL(pWR);
        GoBackR(pWR); // Slave
      }
      else
      {
        GoForvardL(pWR * -1);
        GoForvardR(pWR * -1); // Slave
      }
      
      delay(100);
      pitStop();
   }     
}

void nMotorEncoderTargetRSTurns(int pWR, int trg)
{    
   if(coder[FRIGHT] < trg && coder[RRIGHT] < trg)
   {
      while(coder[FRIGHT] < trg && coder[RRIGHT] < trg)
      {
         if(pWR > 0)
         {
           GoForvardR(pWR);
           GoBackL(pWR);  // Slave
         }
         else
         {
           GoBackR(pWR * -1);
           GoForvardL(pWR * -1);  // Slave
         }       
      }
    
      if(pWR > 0)
      {
        GoBackR(pWR);  // Slave
        GoForvardL(pWR);
      }
      else
      {
         GoForvardR(pWR * -1);  // Slave
         GoBackL(pWR * -1);
      }
      
      delay(100);
      pitStop();      
   }     
}

void aEncoderLeft()
{
  delay(22);
  DropCoders();  
  nMotorEncoderTargetLS(-100, 40);

  delay(22);
  DropCoders();  
  nMotorEncoderTargetRSTurns(-100, 37);
}

void aEncoderRight()
{
  delay(22);
  DropCoders();  
  nMotorEncoderTargetLS(-100, 40);

  delay(22);
  DropCoders();  
  nMotorEncoderTargetRSTurns(100, 37);
}

void aEncoderAroundR()
{
  delay(22);
  DropCoders();  
  nMotorEncoderTargetLS(-100, 40);

  delay(22);
  DropCoders();  
  nMotorEncoderTargetRSTurns(100, 76);
}

void aEncoderAroundL()
{
  delay(22);
  DropCoders();  
  nMotorEncoderTargetLS(-100, 40);

  delay(22);
  DropCoders();  
  nMotorEncoderTargetRSTurns(-100, 76);
}

// **********************************************************************************************
              
void setup()
{

  // Serial.begin(9600);                                // init the Serial port to print the data.  

  DropCoders();
  servoTest.attach(servPin);  
  servoTest.write(0);
  
  attachInterrupt(RLEFT,  LwheelSpeed,  CHANGE);        // init the interrupt mode for the digital pin 0
  attachInterrupt(RRIGHT, RwheelSpeed,  CHANGE);        // init the interrupt mode for the digital pin 1
  attachInterrupt(FLEFT,  FLwheelSpeed, CHANGE);        // init the interrupt mode for the digital pin 2
  attachInterrupt(FRIGHT, FRwheelSpeed, CHANGE);        // init the interrupt mode for the digital pin 3
  
  pinMode(CSr, INPUT);  // Crash sensor right.
  pinMode(CSl, INPUT);  // Crash sensor left.
  
}

void loop()
{  
  // Go around my seeker! 23 itteration.
  // Copacity array is 23 !!! :)
  for(int f = 1000, xR = 0; f <= 2100; f +=50, xR += 8)
  {
        if(xR < 180)
        {
          servoTest.write(xR);     
        }
        else
        {
          servoTest.write(180);
        }
        
      checkDist(f, PWRStart);
    delay(380);
  }
  
  for(int r = 2100, xL = 180; r >= 1000; r -= 50, xL -=8)
  {    
      if(xL < 8)
      {
        servoTest.write(0);
      }
      else
      {
        servoTest.write(xL);
      }
    
      checkDist(r, PWRStart);
    delay(380);
  }  
}

/* 
    This function reads the IR sensor and has the car respond when the range to 
    sensor pin reads is less than the value of irvallarr[] that corresponds 
    with the servo angle. 
*/
void checkDist(int servtime, int PWr)
{
    // Switch crash sensor Right.
    if(digitalRead(CSl) == LOW)
    {
        if(TimeEncode)
          TurnL(PWr);
        else
          aEncoderLeft(); 
    }

    // Switch crash sensor Left.
    if(digitalRead(CSr) == LOW)
    {
       if(TimeEncode)
          TurnR(PWr);
       else
          aEncoderRight();
    }

    // Check voltage on IR analog pin.
    irVal = CheckIPSensor();
    // delay(100);
    // if the voltage on the analog pin is greater than a certain
    // amount (corresponding with a smaller distance) this loop continues.
    while(irVal <= irvalarr[ (servtime - 1000)/50 ] )
    {        
    // the arithmetic involving servtime reduces the
    // servo timing in microseconds to a two-digit
    // number in the irval array.

      // this series of if/else statements checks to
      // see where the servo is positioned and moves the car appropriately.
      if(1400 <= servtime && servtime <= 1550)
      {
        if(TimeEncode)
          turnAroundL(PWr);
        else
          aEncoderAroundL();
      }
      else
        if(1550 < servtime && servtime <= 1700 )
        {          
          if(TimeEncode)
            turnAroundR(PWr);
          else
            aEncoderAroundR();
        }
        else        
          if(1700 < servtime && servtime <= 2100)
          {
            if(TimeEncode)
              TurnR(PWr);
            else  
              aEncoderRight();
          }
          else
            if(1000 <= servtime && servtime < 1400)
            {              
              if(TimeEncode)
               	TurnL(PWr);
              else
                aEncoderLeft();
            }
            else
            { 
              // Debug default case
              GoBack100(PWr);
                delay(100);
                  GoForvard100(PWr);
                    delay(100);
            }

      // This rechecks the distance to continue
      // or break the while loop without moving the servo	
      irVal = CheckIPSensor();
      delay(45);   
    }
      // The car defaults to moving forward in a strait line.
      delay(45);
      GoForvard100(PWr);
}

// Reading the IR sensor.
float CheckIPSensor()
{
  float volts = analogRead(A0) * 0.0048828125;    // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  float distance = 65 * pow(volts, - 1.10);          // worked out from graph 65 = theretical distance / (1/Volts)    
  return (distance);
}

// Encoders functions.
void LwheelSpeed()
{
  coder[RLEFT] ++;  //count the left wheel encoder interrupts
  lastSpeed[0] = coder[RLEFT];
 
  if(lastSpeed[0] >= 32767)
    lastSpeed[0] = 0;
}

void RwheelSpeed()
{
  coder[RRIGHT] ++; //count the right wheel encoder interrupts
  lastSpeed[1] = coder[RRIGHT];

  if(lastSpeed[1] >= 32767)
    lastSpeed[1] = 0;
}

void FLwheelSpeed()
{
  coder[FLEFT] ++;  //count the left wheel encoder interrupts
  lastSpeed[2] = coder[FLEFT];

  if(lastSpeed[2] >= 32767)
    lastSpeed[2] = 0;
}

void FRwheelSpeed()
{
  coder[FRIGHT] ++; //count the right wheel encoder interrupts
  lastSpeed[3] = coder[FRIGHT];

  if(lastSpeed[3] >= 32767)
    lastSpeed[3] = 0;
}

Я не буду полностью описывать всю функцианальность - и раскрывать все карты, пользуйтесь если что то хотите уточнить пишите ваши вопросы!

 

Akatla

Прикрепленные файлы


Сообщение отредактировал akatla: 06 Ноябрь 2016 - 10:34

Sincerely

 

Akatla.


#2 akatla

akatla

    Новичок

  • Пользователи
  • Pip
  • 3 сообщений
  • ГородХабаровск

Отправлено 16 Ноябрь 2014 - 03:57

Тест комментария.


Sincerely

 

Akatla.


#3 Гость_iFlanker_*

Гость_iFlanker_*
  • Гости

Отправлено 17 Ноябрь 2014 - 05:52

Прикольно! Маладца! Давай тест-драйв на "псевдо-триале"!!? ;)



#4 Гость_Akatla_*

Гость_Akatla_*
  • Гости

Отправлено 17 Ноябрь 2014 - 11:08

Прикольно! Маладца! Давай тест-драйв на "псевдо-триале"!!? ;)

Ну, вот я чем и занимаюсь - там еще много интересного будет!



#5 Гость_Борисов Николай_*

Гость_Борисов Николай_*
  • Гости

Отправлено 19 Ноябрь 2014 - 02:30

божественно)

 



#6 hummerr

hummerr

    Новичок

  • Пользователи
  • Pip
  • 1 сообщений

Отправлено 03 Июнь 2015 - 13:05

Молодец! Подробно все расписано! 5+



#7 akatla

akatla

    Новичок

  • Пользователи
  • Pip
  • 3 сообщений
  • ГородХабаровск

Отправлено 10 Ноябрь 2015 - 12:36

Ага, стараюсь блин!


Sincerely

 

Akatla.





Количество пользователей, читающих эту тему: 0

0 пользователей, 0 гостей, 0 анонимных