27.09.2015, 11:14 | #31 |
Junior Member
Регистрация: 22.09.2015
Сообщений: 4
Вес репутации: 0 |
Re: Синхронизация вращения колес у робота
И звените все я разобрался) не с этого я начал теперь едет ровно) спасибо
|
15.12.2015, 08:45 | #32 |
Junior Member
Регистрация: 10.05.2015
Сообщений: 24
Вес репутации: 0 |
Re: Синхронизация вращения колес у робота
Admin, прошу помочь разобраться. Не могу добиться прямолинейности хода. Робота собрал в соответствии с вашей схемой. Использую arduino nano v.7 ,плату arduino, оптопара аналог Вашей TCST1103. С выхода оптопары имею сигнал мин 60мВ и максимум 140мВ, что как мне кажется не достаточно для ардуино. К тому-же хотел бы понять почему Вами была принята данная схема, а не рекомендуемое подключение оптопары к ардуино:
|
15.12.2015, 11:24 | #33 |
Administrator
Регистрация: 12.04.2010
Адрес: Москва
Сообщений: 9,618
Вес репутации: 9823 |
Re: Синхронизация вращения колес у робота
У меня практически такая же схема, я только для экономии одного резистора включил светодиоды последовательно
|
15.12.2015, 11:35 | #34 |
Junior Member
Регистрация: 10.05.2015
Сообщений: 24
Вес репутации: 0 |
Re: Синхронизация вращения колес у робота
Что светодиоды последовательно, это я понял. Главный вопрос почему нет резистора и самой цепи питания в коллекторе фототранзистора? При этом бы и обеспечивалась напряжение на выходе оптопары макс 5В и мин 0В.
|
15.12.2015, 11:39 | #35 |
Administrator
Регистрация: 12.04.2010
Адрес: Москва
Сообщений: 9,618
Вес репутации: 9823 |
Re: Синхронизация вращения колес у робота
Он подключается программно, тот который внутри контроллера
|
15.12.2015, 13:36 | #36 |
Junior Member
Регистрация: 10.05.2015
Сообщений: 24
Вес репутации: 0 |
Re: Синхронизация вращения колес у робота
Получается, что у меня не подключается? А Вы можете померить на своем роботе напряжение на D2 и D3 максимальное и минимальное при вращении энкодера? И может быть подскажите еще как можно проверить его работу.
|
06.12.2016, 02:04 | #37 |
Member
Регистрация: 06.12.2016
Сообщений: 35
Вес репутации: 0 |
Re: Синхронизация вращения колес у робота
Я думаю, у вас в программе надо исправить - прерывания должны сниматься с экнодеров и в поворотах, только в поворотах не надо выравнивать движение. Зато по разности счетчиков и разности управляющей скорости на внешнем и внутреннем колеах можно четко вычислять угол поворота.
|
06.12.2016, 09:02 | #38 |
Administrator
Регистрация: 12.04.2010
Адрес: Москва
Сообщений: 9,618
Вес репутации: 9823 |
Re: Синхронизация вращения колес у робота
В данной теме приведен пример, само собой углы поворотов можно задавать разностью импульсов в переменной course, желательно иметь две переменные по одной на каждое колесо
|
06.12.2016, 11:36 | #39 | |
Member
Регистрация: 06.12.2016
Сообщений: 35
Вес репутации: 0 |
Re: Синхронизация вращения колес у робота
Цитата:
А в поворотах уже считать реальные показания счетчиков и вычислять угол поворота, исходя из ширины базы робота, диаметра колеса и скорости внутреннего колеса. Т.е. получается, что IntOn() должна включаться всегда, как какой-то двигатель начинает движение (чтобы не дублировать надо ввести булевую переменную, отражающую включена функция или нет), а IntOff() - когда оба двигателя останавливаются и при этом обнулять оба счетчика (левый и правый) и присваивать false той самой введенной переменной. Последний раз редактировалось Paul_B; 06.12.2016 в 11:46. |
|
08.12.2016, 01:11 | #40 |
Member
Регистрация: 06.12.2016
Сообщений: 35
Вес репутации: 0 |
Re: Синхронизация вращения колес у робота
Вот какие есть задумки по движению с энкодерами... Можно задавать повороты в градусах, а движения в сантиметрах.
Робота пока жду, поэтому только в теории. Код:
#include <MsTimer2.h> //библиотека для работы с таймером-2 (для опроса ИК-датчиков и отсчета времени #include <Servo.h> //используем библиотеку для работы с сервоприводом #include <avr/wdt.h> //подключение библиотеки для работы со сторожевым таймером #include <IRremote.h> //библиотека для ИК приемника #define size_buff 5 #define trigPin 12 #define echoPin 11 #define ServoF 10 #define ML_E 2 // энкодер левого двигателя #define MR_E 3 // энкодер правого двигателя #define ML_1 4 // левый двигатель #define ML_2 5 #define ML_S 6 #define MR_1 7 // правый двигатель #define MR_2 8 #define MR_S 9 #define Sound 13 Servo Servo_F; //объявляем переменную servo типа Servo uint8_t U_Servo; // активный угол серво char Napr[2] = {' ', ' '}; //выполняемое движение ячейка [0] - задает направление F/B, ячейка [1] - поворот R/L uint8_t SPEED, OLD_SPEED, //переменные для текущей скорости и прошлой скорости L_SPD = 50, // медленная скорость M_SPD = 150, // средняя скорость F_SPD = 255; // максимальная скорость bool LR_count = false; // флаг работы счетчиков энкодеров bool STOP_L=false, STOP_R=false; // признак притормаживания соотв. колеса (для энкодеров) uint16_t ML_count, MR_count; // счетчики правого и левого энкодеров uint16_t LimD_count, // предельное значение счетчика, на котором при движении вперед нет препятствий LimF_count, // предельное значение счетчика, если задан отрезок прямолинейного движения LimL_count, // значение счетчика левого колеса при правом повороте на заданный угол LimR_count; // значение счетчика правого колеса при левом повороте на заданный угол uint16_t TurnL_count=0, TurnR_count=0; // счетчик завершенного поворота uint16_t sensor[size_buff], R_time; uint16_t IR[4] = {A2, A3, A4, A5}; uint16_t IR_Dist[4]; // для расчета расстояний из счетчика энкодера uint16_t BAZA = 120; //ширина между колесами в mm (середин колес) uint16_t KOLESO = 65; //диаметр колеса в mm uint16_t N_Encoder = 20; //количество отверстий в энкодере float Pi = 3.1415926; void setup() { uint8_t i; Serial.begin(9600); // инициализация послед. порта Servo_F.attach(ServoF); Servo_Ugol('F', 90, 1); pinMode(trigPin, OUTPUT); // триггер - выходной пин pinMode(echoPin, INPUT); // эхо - входной pinMode (ML_1, OUTPUT); pinMode (ML_2, OUTPUT); pinMode (ML_S, OUTPUT); pinMode (ML_E, INPUT); pinMode (MR_1, OUTPUT); pinMode (MR_2, OUTPUT); pinMode (MR_S, OUTPUT); pinMode (MR_E, INPUT); pinMode (Sound, OUTPUT); for (i = 0; i < 4; i++) pinMode(IR[i], INPUT); R_time = 0; SPEED = 0; MsTimer2::set(500, IR_Move()); // прерывания по таймеру, период 500 мс MsTimer2::start(); // разрешение прерывания IntOff(); R_Stop(); for (i = 0; i < 12; i++) beep(70, random(100, 2000)); //звуковое оповещение готовности робота wdt_enable(WDTO_1S); Serial.println("Baster ready!"); } void loop() { // put your main code here, to run repeatedly: if (Serial.available()) { Serial.read(); } if (Napr[1] == 'E' || ML_count >= LimD_count) // закончился поворот или уже долго едем без проверки УЗД направления { Napr[1] = ' '; if (Napr[0] == 'F') SPEED=Forward_Distance(SPEED); } //uint16_t distance = GetDistance(); // получаем дистанцию с датчика //Serial.println(distance); // выводим в последовательный порт // delay(100); wdt_reset(); } bool Servo_Ugol(char Serv, uint16_t ugol, bool Wt) { if (Serv == 'F') { if (U_Servo == ugol) exit(0); Servo_F.write(ugol); //ставим вал под ugol if (Wt) delay(20+int(120 * abs(U_Servo - ugol) / 60)); // ждем пока вал повернется U_Servo = ugol; } } //************* определение минимального расстояния в секторе поворота / разворота ************* uint16_t GetSector(char Serv, uint16_t ugol) { uint16_t i, dist, min_dist = 300; unsigned long t0,t1; t0=millis()+200; Servo_Ugol(Serv, ugol, false); // начинаем поворачивать серву do { // и во время поворота снимаем данные с УЗД dist = GetDistance(); if(dist<min_dist) min_dist=dist; // самое минимальное расстояние отсылаем t1=millis(); } while (t1<t0); return(min_dist); } //************ Определение дистанции до объекта в см *************** uint16_t GetDistance() { uint16_t dist; uint8_t i; digitalWrite(trigPin, LOW); delayMicroseconds(2); for (i = 0; i < size_buff; i++) //производим несколько замеров { digitalWrite(trigPin, HIGH); // генерируем импульс запуска delayMicroseconds(10); digitalWrite(trigPin, LOW); dist = pulseIn(echoPin, HIGH, 12000); //считываем длительность времени прохождения эха, ограничить время ожидания if (dist == 0) dist = 12000; else delayMicroseconds(20); sensor[i] = dist; //сохранить в массиве } dist = (find_similar(sensor, size_buff, 58)) / 58; // //фильтруем показания датчика и переводим в см return dist; } //**************Поиск макс повторяющегося элемента в массиве **************************** uint16_t find_similar(uint16_t *buf, uint8_t s_buff, uint8_t range) { uint8_t maxcomp = 0; //счётчик максимального количества совпадений uint16_t mcn = 0; //максимально часто встречающийся элемент массива uint16_t comp; //временная переменная uint8_t i, j, n; //счётчик совпадении uint8_t i_numb, i_mas[size_buff]; // индекс интервала и показатель принадлежности к заданному интервалу отклонений range++; //допустимое отклонение for (i = 0; i < s_buff; i++) i_mas[i] = 0; // задаем начальные нули. чтобы сэкономить можно задать static uint8_t i_mas[256]; i_numb = 1; // задаем номер интервала. если измерение будет лежать в этом интервале, то в индексном массиве присваиваем этот номер for (i = 0; i < s_buff; i++) if (i_mas[i] == 0) // если элемент не попал ни в чей интервал { comp = buf[i]; //кладем элемент массива в comp n = 0; //счётчик совпадении for (j = i; j < s_buff; j++) if (i_mas[j] == 0) // если раньше этот элемент не попал в какой-то интервал, то его рассматриваем { if (buf[j] > comp - range) if (buf[j] < comp + range) { i_mas[j] = i_numb; n++; } } if (n > maxcomp) //если число повторов больше чем было найдено ранее { maxcomp = n; //сохраняем счетяик повторов mcn = comp; //сохраняем повторяемый элемент } i_numb++; // тут в итоге будет количество непересекающихся интервалов измерений } return mcn; } //************* прозвон через УЗД беспрепятственной дистанции вперед *********** int Forward_Distance(uint8_t spd) { int s; Servo_Ugol('F', 90, true); uint16_t dist = GetDistance(); // замеряем расстояние до препятствия по прямой LimD_count = max(0,ML_count + N_Encoder * 10 * (dist - 20) / (Pi * KOLESO)); //сколько можно ехать по прямой без УЗД if (dist < 100) s = min(M_SPD, spd); if (dist < 50) s = min(L_SPD, spd); if (dist < 20) s=-1; return (s); } //********* реакция на сработавшие ИК-датчики int IR_Move() { IR_Distance(); // .... return (0); } //********* опрос ИК-датчиков (в т.ч. и по таймеру каждые 500мс) ************ void IR_Distance() { uint16_t dist; uint8_t i, j; R_time++; // счетчик времени с начала включения for (j = 0; j < 4; j++) { for (i = 0; i < size_buff; i++) //производим несколько замеров { dist = analogRead(IR[j]); // чтение напряжения на входе A3 //обработка показанийб чтобы привести к сантиметрам sensor[i] = dist; //сохранить в массиве } //IR_Dist[j]=find_similar(sensor, size_buff, 1); // //фильтруем показания датчика и переводим в см } } void MotorL() // обработка внешнего прерывания левое колесо { ML_count++; if (SPEED == 0) exit(0); if(LimF_count>0) // есть ограничения на езду прямо if(ML_count>=LimF_count) { LimF_count=0; R_Stop(); } if (Napr[1] == ' ') //когда не поворачиваем притормаживаем правый двигатель { if (MR_count - ML_count > 1) // т.е. правый двигатель крутится быстрее и { if(!STOP_L) {// подтормаживаем правый двигатель, если не подтормжен левый analogWrite (MR_S, int(SPEED / 2));// притормаживаем правый двигатель STOP_R=true; } } else {// курс ровный if(STOP_R) // если правый двигатель тормозился, возвращаем прежнюю сорость { analogWrite (MR_S, SPEED); STOP_R=false; } } } else { if(Napr[1]=='R') // задан поворот на фикированный угол { if(ML_count-MR_count>=LimL_count) // поворот на заданный градус завершен { TurnL_count=0; LimL_count=0; analogWrite (MR_S, SPEED); // восстанавливаем скорость на правом двигателе Napr[1]=='E'; // признак окончания поворота ML_count = 0; // начало прямолинейного движения, скидываем счетчики MR_count = 0; } else TurnL_count=MR_count-ML_count; // если поворачиваем, считаем на столько осуществлен поворот } if(Napr[1]=='Z') // задан разворот { if(ML_count>=LimL_count) // поворот на заданный градус завершен { LimL_count=0; R_Stop(); } } } } void MotorR() // обработка внешнего прерывания правое колесо { MR_count++; if (SPEED == 0) exit(0); if(LimF_count>0) // есть ограничения на езду прямо if(MR_count>=LimF_count) { LimF_count=0; R_Stop(); } if (Napr[1] == ' ') //когда не поворачиваем притормаживаем левый двигатель { if (ML_count - MR_count > 1) // т.е. левый двигатель крутится быстрее и { if(!STOP_R) { // подтормаживаем левый двигатель analogWrite (ML_S, int(SPEED / 2)); STOP_L=true; } } else {// курс ровный if(STOP_L) // если левый двигатель тормозился, возвращаем прежнюю сорость { analogWrite (ML_S, SPEED); STOP_L=false; } } } else { if(Napr[1]=='L') // задан поворот на фикированный угол { if(MR_count-ML_count>=LimR_count) // поворот на заданный градус завершен { TurnR_count=0; LimR_count=0; analogWrite (ML_S, SPEED); // восстанавливаем скорость на левом двигателе Napr[1]=='E'; // признак окончания поворота ML_count = 0; // начало прямолинейного движения, скидываем счетчики MR_count = 0; } else TurnR_count=MR_count-ML_count; // если поворачиваем, считаем на столько осуществлен поворот } } } //********* остановка робота ************************ void R_Stop() { if (LR_count) IntOff(); //выключаем счетки энкодеров analogWrite(ML_S, 0); // останавливаем двигатели analogWrite(MR_S, 0); Napr[0]==' '; Napr[1]==' '; OLD_SPEED = SPEED; SPEED = 0; } //*********** установка двигателей на движение вперед *** void Set_Forward() { if (Napr[0] != 'F') { digitalWrite (ML_1, LOW); // устанавливаем направление левого и правого двигателя digitalWrite (ML_2, HIGH); digitalWrite (MR_1, LOW); digitalWrite (MR_2, HIGH); Napr[0] = 'F'; //задаем символьное направление движения } } //*********** установка двигателей на движение назад *** void Set_Back() { if (Napr[0] != 'B') { digitalWrite (ML_1, HIGH); // устанавливаем направление левого и правого двигателя digitalWrite (ML_2, LOW); digitalWrite (MR_1, HIGH); digitalWrite (MR_2, LOW); Napr[0] = 'B'; //задаем символьное направление движения } } //*********** движение робота назад либо на заданное число см, либо до упора ********* int R_Back(uint8_t spd, uint16_t len) { if(Napr[0]!='B') { ML_count = 0; // начало прямолинейного движения, скидываем счетчики MR_count = 0; Set_Back(); //устанавливаем двигатели для движения назад } LimF_count = 0; if (len > 0) // если задано расстояние для движения назад LimF_count = int(N_Encoder * 10.0 * len / (Pi * KOLESO)); // значение счетчика для заданного расстояния Fast_slow(spd); //плавно разгоняемся // ..... return (0); } //*********** движение робота вперед либо на заданное число см, либо до упора ******** int R_Forward(uint8_t spd, uint16_t len) { int s; if(Napr[0]!='F') { ML_count = 0; // начало прямолинейного движения, скидываем счетчики MR_count = 0; Set_Forward(); //устанавливаем двигатели для движения вперед } if (Napr[0] != 'F' || Napr[1] != ' ' || ML_count >= LimD_count) { s = Forward_Distance(spd); if (s == -1) return (-1); spd = s; } LimF_count=0; if (len > 0) // если задано расстояние для движения вперед LimF_count = int(N_Encoder * 10.0 * len / (Pi * KOLESO)); // значение счетчика для движения а заданное расстояние Fast_slow(spd); //плавно разгоняемся // .... return (0); } //*********** замедление или разгон с одной скорости на другую ********* uint8_t Fast_slow(uint8_t spd) { uint8_t i, max_spd, min_spd, sh; unsigned long t0,t1; if (!LR_count) IntOn(); //включаем енкодеры, если они отключены if (spd == SPEED) return (spd); //если скорость не меняется, выходим из процедуры OLD_SPEED = SPEED; //сохраняем значение старой скорости if(Napr[1]==' ') // ксли не задан поворот, то плавно меняем изменение скорости { if (spd > SPEED) { max_spd = spd; min_spd = SPEED; } else { max_spd = SPEED; min_spd = spd; } if (SPEED >= M_SPD || spd >= M_SPD) // и скорость задана от средней, плавно увеличиваем или уменьшаем скорость { sh = (max_spd - min_spd) / 10; for (i = 1; i < 10; i++) { analogWrite (ML_S, min_spd + i * sh); analogWrite (MR_S, min_spd + i * sh); t0=millis()+100; do { // нужно вставить опрос ИК-датчиков при разгоне, чтобы не врезаться t1=millis(); } while (t1<t0); } } } analogWrite (ML_S, spd); analogWrite (MR_S, spd); SPEED = spd; return (spd); } //*************** поворот налево ******************* int R_Left(uint16_t ugol, float Radius) // Radius - задает радиус движения по окружности при повороте // ugol - задает дугу окружности. { if (SPEED == 0) return(-2); //если робот не движется, поворота не будет // задается угол поворота. если угол зада 0, то включаетсяя поворот до тех пор, пка не будет принудительный выход из поворота if (!LR_count) IntOn(); Napr[1] = 'L'; if(ugol==0) ugol=360; if (Napr[0] == 'F') Servo_Ugol('F', max(0, 90 - int(ugol / 2)), false); analogWrite (ML_S, int(Radius*SPEED)); // тормозим левый двигатель LimR_count = int((ugol / 360.0) * N_Encoder * 2 * BAZA / KOLESO); // значение счетчика для поворота на заданный угол } int R_Right(uint16_t ugol, float Radius) // Radius - задает радиус движения по окружности при повороте // ugol - задает дугу окружности. { if (SPEED == 0) return(-2); //если робот не движется, поворота не будет // задается угол поворота. если угол зада 0, то включаетсяя поворот до тех пор, пка не будет принудительный выход из поворота if (!LR_count) IntOn(); Napr[1] = 'R'; if(ugol==0) ugol=360; if (Napr[0] == 'F') Servo_Ugol('F', min(180, 90 + int(ugol / 2)), false); analogWrite (MR_S, int(Radius*SPEED)); // тормозим правый двигатель LimL_count = int((ugol / 360.0) * N_Encoder * 2 * BAZA / KOLESO); } //******** Разворот на месте на угол (0-360) в заданную сторону ('L' 'R') ********** int Rotation(uint16_t ugol, char Side) { uint16_t dist, i = 0; float x; R_Stop(); if (!LR_count) IntOn(); i = 0; while (i < 2) { if (Side == 'L') { dist = GetSector('F',0); // замеряем расстояние до препятствия в левом секторе if (dist < 15) { Servo_Ugol('F', 90, true); // нашли препятствие, возвращаем серву в центр и пробуем развернуться через право Side = 'R'; ugol = 360 - ugol; i++; } else break; } if (Side == 'R') { dist = GetSector('F',180); // замеряем расстояние до препятствия в правом секторе if (dist < 15) { // нашли препятствие, возвращаем серву в центр и пробуем развернуться через право if(i==0) { // если еще не пробовали разворот через лево, возвращаем серву в центр и пробуем развернуться через лево Side = 'L'; ugol = 360 - ugol; Servo_Ugol('F', 90, true); // нашли препятствие, возвращаем серву в центр и пробуем развернуться через право } i++; } else break; } } Servo_Ugol('F', 90, false); if (i == 2) return (-1); // через обе стороны нельзя развернуться if (Side == 'L') //разворачиваемся через лево { digitalWrite (ML_1, HIGH); digitalWrite (ML_2, LOW); digitalWrite (MR_1, LOW); digitalWrite (MR_2, HIGH); } if (Side == 'R') { //разворачиваемся через право digitalWrite (ML_1, LOW); digitalWrite (ML_2, HIGH); digitalWrite (MR_1, HIGH); digitalWrite (MR_2, LOW); } Napr[1] = 'Z'; LimL_count = (ugol / 360.0) * N_Encoder * BAZA / KOLESO; analogWrite (ML_S, int((L_SPD + M_SPD) / 2)); // разворачиваемся на средней скорости analogWrite (MR_S, int((L_SPD + M_SPD) / 2)); R_Stop(); return (0); } //*************************************************** void IntOn() { LR_count = true; attachInterrupt(0, MotorL, RISING); // настроить срабатывание прерывания interrupt0 pin 2 по перерпапду с низкого (Low) на высокий(HIGH) attachInterrupt(1, MotorR, RISING); // настроить срабатывание прерывания interrupt1 pin 3 по перерпапду с низкого (Low) на высокий(HIGH) } void IntOff() { LR_count = false; detachInterrupt(0); // отключить срабатывание прерывания interrupt0 detachInterrupt(1); // отключить срабатывание прерывания interrupt1 } //***************Beep от 50 до 2000Гц**************** void beep(uint16_t dur, uint16_t frq) { uint16_t i; //dur=(1000/frq)*dur; //расчет длительности бипа uint16_t per = 500000 / frq; //длит. полупер в мкс dur = dur / (per / 250) * 2; for (i = 0; i < dur; i++) { digitalWrite(Sound, HIGH); delay(per); digitalWrite(Sound, LOW); delay(per); } } //**************програмный сброс******************* void reset() { wdt_disable(); //отключить, так как он может быть уже включен wdt_enable(WDTO_15MS); //установим минимально возможное время 15мс while (1) {} //зациклить } Последний раз редактировалось Paul_B; 11.12.2016 в 00:32. |
Здесь присутствуют: 8 (пользователей: 0 , гостей: 8) | |
|
|