Зависание робота
- Войдите на сайт для отправки комментариев
Чт, 23/04/2015 - 21:35
Добрый вечер!
Прошу помощи. Делаю потихоньку робота, взяв за основу проект Евгения(http://arduino.ru/forum/proekty/moi-kolesnyi-apparat-na-arduino-mega). Платформа гусенечная. В данный момент тестирую радиоуправление. Столкнулся с проблемой в какой-то момент передачи робот зависает: не смотря на то что отпускаю джойстик, робот продолжает движение, пока не дашь другую комманду. В мониторе порта передатчик ведет себя как надо. Робот же, при зависании, не получает комманду стоп. Собственно, просьба помочь разобраться. Код передатчика и робота прилагаю.
Передатчик
[code]
#include <VirtualWire.h>
int VRx; //ось х джойстика
int VRy; //ось у джойстика
int SW;
unsigned long curTime;
char act, prevAct; //команда
char *msg = "****"; //кодовое слово
void setup()
{
vw_set_tx_pin(5);
Serial.begin(9600);
vw_set_ptt_inverted(true);
vw_setup(2000); // Bits per sec
}
void loop()
{
VRx = analogRead(0);
VRy = analogRead(1);
act = 'S';
msg = "****S";
curTime = millis();
if(VRx < 100)
{
act = 'B';
msg = "****B";
curTime = millis();
}
if(VRx > 900)
{
act = 'F';
msg = "****F";
curTime = millis();
}
if(VRy < 100)
{
act = 'R';
msg = "****R";
curTime = millis();
}
if(VRy > 900)
{
act = 'L';
msg = "****L";
curTime = millis();
}
if(prevAct != act)
{
Serial.println(act);
vw_send((uint8_t *)msg, strlen(msg));
vw_wait_tx();
}
if(( millis() - curTime) > 500)
act = '?';
prevAct = act;
delay(200);
}
[/code]
Робот
[code]
#include <VirtualWire.h>
#define Motor1Dvig1 47
#define Motor1Dvig2 49
#define Motor2Dvig1 51
#define Motor2Dvig2 53
#define RegSpeed1 9
#define RegSpeed2 8
#define KsenonL 4
#define KsenonR 5
long prevTime = 0; //для алгоритма работы цикла loop() 10р/с
long deltaTime = 0; //для алгоритма работы цикла loop() 10р/с
char passRfCtrl[4] = {'*', '*', '*', '*'}; //кодовое слово для проверки приходящего сообщения
char act = '?'; //текущая команда
void setup()
{
pinMode(Motor1Dvig1, OUTPUT);
pinMode(Motor1Dvig2, OUTPUT);
pinMode(Motor2Dvig1, OUTPUT);
pinMode(Motor2Dvig2, OUTPUT);
pinMode(KsenonL, OUTPUT);
pinMode(KsenonR, OUTPUT);
pinMode(RegSpeed1, OUTPUT);
pinMode(RegSpeed2, OUTPUT);
digitalWrite(KsenonL, LOW);
digitalWrite(KsenonR, LOW);
Serial.begin(9600);
vw_set_rx_pin(6);
vw_set_ptt_inverted(true); // Required for DR3100
vw_setup(2000); // Bits per sec
vw_rx_start();
digitalWrite(KsenonL, HIGH);
digitalWrite(KsenonR, HIGH);
}
//Поворот вправо на месте========
void turnR()
{
digitalWrite(KsenonR, HIGH);
digitalWrite(Motor1Dvig1, HIGH);
digitalWrite(Motor1Dvig2, LOW);
digitalWrite(Motor2Dvig1, HIGH);
digitalWrite(Motor2Dvig2, LOW);
analogWrite(RegSpeed1, 250);
analogWrite(RegSpeed2, 250);
}
//Поворот влево на месте========
void turnL()
{
digitalWrite(KsenonL, HIGH);
digitalWrite(Motor1Dvig1, LOW);
digitalWrite(Motor1Dvig2, HIGH);
digitalWrite(Motor2Dvig1, LOW);
digitalWrite(Motor2Dvig2, HIGH);
analogWrite(RegSpeed1, 250);
analogWrite(RegSpeed2, 250);
}
//Прямо!!!=========================
void goF()
{
digitalWrite(Motor1Dvig1, LOW);
digitalWrite(Motor1Dvig2, HIGH);
digitalWrite(Motor2Dvig1, HIGH);
digitalWrite(Motor2Dvig2, LOW);
analogWrite(RegSpeed1, 150);
analogWrite(RegSpeed2, 150);
}
//Назад===========================
void goB()
{
digitalWrite(Motor1Dvig1, HIGH);
digitalWrite(Motor1Dvig2, LOW);
digitalWrite(Motor2Dvig1, LOW);
digitalWrite(Motor2Dvig2, HIGH);
analogWrite(RegSpeed1, 150);
analogWrite(RegSpeed2, 150);
}
//Стоим и курим))))
void turnS()
{
analogWrite(RegSpeed1, 0);
analogWrite(RegSpeed2, 0);
digitalWrite(KsenonR, LOW);
digitalWrite(KsenonL, LOW);
digitalWrite(Motor1Dvig1, LOW);
digitalWrite(Motor1Dvig2, LOW);
digitalWrite(Motor2Dvig1, LOW);
digitalWrite(Motor2Dvig2, LOW);
}
void loop()
{
prevTime = millis();
uint8_t buf[VW_MAX_MESSAGE_LEN];
uint8_t buflen = VW_MAX_MESSAGE_LEN;
//----------------------------------------------------- RF control
if (vw_get_message(buf, &buflen)) // Non-blocking
{
int i;
char letter;
for (i = 0; i < buflen-1; i++)
{
letter = buf[i];
}
letter = buf[buflen-1];
Serial.println(letter);
switch (letter)
{
case 'S':
act = 'S';
break;
case 'B':
act = 'B';
break;
case 'F':
act = 'F';
break;
case 'R':
act = 'R';
break;
case 'L':
act = 'L';
break;
case 'W':
act = 'W';
break;
}
}
switch (act)
{
case 'S':
turnS();
break;
case 'B':
goB();
break;
case 'F':
goF();
break;
case 'R':
turnR();
break;
case 'L':
turnL();
break;
}
// --------------------------------------------------- Final Section
act = '?';
deltaTime = millis() - prevTime;
if(deltaTime < 100)
delay(100 - deltaTime);
}
[/code]
Никто не подскажет? Может, хотя бы , в каком направлении работать?
На 433 частоте очень много помех, тем более на АМ модуляции.
Нужно при отпускании джойстика передавать команду стоп, не постоянно, а несколько раз. Глушить автосигналки тоже постоянно не рекомендуется :)
На приемнике нужно запускать отсчет времени при приеме команды, если прквышаем лимит то останавливаем машинку, это на случай потери связи и зависания, также нужен сторожевой таймер от зависания. Только запустить его на ардуине это танцы с бубном :)
Скорость не регулируется в коде, почему?
Добрый день!
Спасибо. Код передатчика изменил, теперь постоянно шлёт стоп, если нет другой комманды. Другой вариант: постоянно шлет стоп, а если дана комманда- с такой же частотой шлёт комманду, что наверно удобнее для таймера. Как времечко будет таймер времени приема комманды тоже сделаю. А вот со сторожевым таймером наверное будет проблема - пока не дорос до танца с бубном(( Стоит Мега, а читал, что по умолчании работоет только в Уно...
Чтобы постоянно стоп не слать, прописываете флаг команды стоп. При выбросе конанд движения подымаете флаг. Как отпустили джойстик, проверяете флаг на еденичку, если да - то посылаете 10 команд стоп и сбрасываете флаг.
Не нужно постояно слать стоп, хотя бы для сохранения батарей пульта. Про нервы автолюбителей, чьи сигналки не будут их слушаться из-за вашего пульта я промолчу.
По отключению двигателей при прекращении команд можно так написать
if(!flag_stop_motor & (millis() - time_no_comand) > 1000) { flag_stop_motor = 1; left_motor = 0; riht_motor = 0; analogWrite(M1, 0x00); // выключаем моторы, если нет команд с пульта более 250 миллисекунд analogWrite(M2, 0x00); digitalWrite(D1, forward); // Задаем направление вращения digitalWrite(D2, forward); }при приеме очередой команды засекается time_no_comand
time_no_comand = millis(); //засекаем время после прилета команды
Подскажите:
vw_get_message(buf, &buflen)это приём комманды?
Если не затруднит, можешь скинуть исправленный вариант кода? Просто занимаюсь тем же самым. И проблема идентична)
Добрый день!
К сожалению с временем беда, до реализации руки пока не доходят. Как что-то обрисуется - напишу.
часто бывает джой при отпускании не переходит в нейтральное положение.
выведи джойстик в сериал и проверь как он работает
Столкнулся с проблемой в какой-то момент передачи робот зависает: не смотря на то что отпускаю джойстик, робот продолжает движение, пока не дашь другую комманду.
Почему проблема ?
Я так и задумывал и код так написан. До тех пор пока не пришла новая команда, робот едет по старой. Постоянно джойстик жать не надо.
А команды стоп вообще нет, если хотите можете задействовать SW (джойстик нажимается как кнопка)
Код немного подкорректирован и при возвращение джойстика в нейтраль - посылается команда стоп. Т.е. отпустил джойстик остановился. Часто происходит - отпустил джойстик, робот продолжает движение. Даже при повторном нажатии джойстика останавливается не сразу. Передача с джойстика в этот момент идёт, во всяком случае монитор порта показывает.
Добрый вечер! Опять вернулся к своему робокопу)) Видимо без помощи мне не справится...Но все по порядку.
1.Пульт управления сделал как надо -шлет команду, пока держишь рычаг джойстика , повторяет комманду. После отпускания - три разв стоп.
Код пульта
[code] #include <VirtualWire.h> int i; int flagStop = 0; int VRx; //ось х джойстика int VRy; //ось у джойстика int SW; int Repeat = 0; unsigned long curTime; char act, prevAct; //команда char *msg = "****"; //кодовое слово void setup() { vw_set_tx_pin(5); Serial.begin(9600); vw_set_ptt_inverted(true); vw_setup(2000); // Bits per sec } void flagS() { act = 'S'; msg = "****S"; curTime = millis(); Serial.println(act); vw_send((uint8_t *)msg, strlen(msg)); vw_wait_tx(); } void loop() { label: VRx = analogRead(0); VRy = analogRead(1); if(VRx < 100) { flagStop = 1; act = 'B'; msg = "****B"; curTime = millis(); Serial.println(act); vw_send((uint8_t *)msg, strlen(msg)); vw_wait_tx(); goto label; } else if (VRx > 900){ flagStop = 1; act = 'F'; msg = "****F"; curTime = millis(); Serial.println(act); vw_send((uint8_t *)msg, strlen(msg)); vw_wait_tx(); goto label; } else if (VRy < 100){ flagStop = 1; act = 'R'; msg = "****R"; curTime = millis();Serial.println(act); vw_send((uint8_t *)msg, strlen(msg)); vw_wait_tx(); goto label; } else if (VRy > 900){ flagStop = 1; act = 'L'; msg = "****L"; curTime = millis(); Serial.println(act); vw_send((uint8_t *)msg, strlen(msg)); vw_wait_tx(); goto label; } if (flagStop == 1){ for(i=0; i <= 3; i++){ flagS();} } flagStop = 0; } [/code]С роботом думал разобрался - при работе от USB работал идеально. Итог - были куплен другой источник питания(до этого была крона, которая питала только ардуино, но, видимо что-то не шло.)
Все бы не чего, пока не решил привязать передачу комманды, которая пускала робота в автоматическое курсирование по комнате, полагаясь только на УЗ дальномеры. При переходе на ручное управление - начались зависания.
Собственно, прошу, ткните пальцем в код, где нужно записать проверку пришла комманда или нет, а то что-то не разобраться.
Код робота без фунции с дальномерами. Где в комментариях filtr - жалкие попытки добавить проверку.
[code] #include <VirtualWire.h> #define Motor1Dvig1 47 #define Motor1Dvig2 49 #define Motor2Dvig1 51 #define Motor2Dvig2 53 #define RegSpeed1 9 #define RegSpeed2 8 #define KsenonL 4 #define KsenonR 5 int i; long prevTime = 0; //для алгоритма работы цикла loop() 10р/с long deltaTime = 0; //для алгоритма работы цикла loop() 10р/с char passRfCtrl[4] = {'*', '*', '*', '*'}; //кодовое слово для проверки приходящего сообщения char act = '?'; //текущая команда int flag_stop_motor = 0;//filtr long time_no_command;//filtr void setup() { pinMode(Motor1Dvig1, OUTPUT); pinMode(Motor1Dvig2, OUTPUT); pinMode(Motor2Dvig1, OUTPUT); pinMode(Motor2Dvig2, OUTPUT); pinMode(KsenonL, OUTPUT); pinMode(KsenonR, OUTPUT); pinMode(RegSpeed1, OUTPUT); pinMode(RegSpeed2, OUTPUT); digitalWrite(KsenonL, LOW); digitalWrite(KsenonR, LOW); Serial.begin(9600); vw_set_rx_pin(6); vw_set_ptt_inverted(true); // Required for DR3100 vw_setup(2000); // Bits per sec vw_rx_start(); } //Поворот влево на месте======== void turnL() { digitalWrite(KsenonL, HIGH); digitalWrite(Motor1Dvig1, HIGH); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, HIGH); digitalWrite(Motor2Dvig2, LOW); analogWrite(RegSpeed1, 120); analogWrite(RegSpeed2, 120); } //Поворот вправо на месте======== void turnR() { digitalWrite(KsenonR, HIGH); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, HIGH); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, HIGH); analogWrite(RegSpeed1, 120); analogWrite(RegSpeed2, 120); } //Вперед========================= void goF() { digitalWrite(KsenonR, HIGH); digitalWrite(KsenonL, HIGH); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, HIGH); digitalWrite(Motor2Dvig1, HIGH); digitalWrite(Motor2Dvig2, LOW); for(i=0; i <= 255; i++){ analogWrite(RegSpeed1, i); analogWrite(RegSpeed2, i);} } //Назад=========================== void goB() { digitalWrite(Motor1Dvig1, HIGH); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, HIGH); for(i=0; i <= 255; i++){ analogWrite(RegSpeed1, i); analogWrite(RegSpeed2, i);} } //Стоим и курим)))) void turnS() { analogWrite(RegSpeed1, 0); analogWrite(RegSpeed2, 0); digitalWrite(KsenonR, LOW); digitalWrite(KsenonL, LOW); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, LOW); } void loop() { prevTime = millis(); uint8_t buf[VW_MAX_MESSAGE_LEN]; uint8_t buflen = VW_MAX_MESSAGE_LEN; //----------------------------------------------------- RF control if (vw_get_message(buf, &buflen)) // Non-blocking { time_no_command = millis(); //filtr int i; char letter; for (i = 0; i < buflen-1; i++) { letter = buf[i]; } letter = buf[buflen-1]; Serial.println(letter); switch (letter) { case 'S': act = 'S'; break; case 'B': act = 'B'; break; case 'F': act = 'F'; break; case 'R': act = 'R'; break; case 'L': act = 'L'; break; case 'W': act = 'W'; break; } } switch (act) { case 'S': turnS(); break; case 'B': goF(); break; case 'F': goB(); break; case 'R': turnR(); break; case 'L': turnL(); break; } if(flag_stop_motor == 0 & (millis() - time_no_command) > 1000)//filtr { flag_stop_motor = 1;//filtr turnS();}//filtr else { flag_stop_motor = 0;//filtr } // --------------------------------------------------- Final Section } [/code]И код с дальномерами.(Кстати в данном исполнении левый дальномер работает плохо, но версия кода ещё сырая))
[code] //В эта программа, аналогично 30_07 //Добаляю W Автоматическое движение #include <VirtualWire.h> #define Motor1Dvig1 47 #define Motor1Dvig2 49 #define Motor2Dvig1 51 #define Motor2Dvig2 53 #define RegSpeed1 9 #define RegSpeed2 8 #define KsenonL 4 #define KsenonR 5 #define EchoL 24 #define EchoR 28 #define TrigL 26 #define TrigR 30 int i; long prevTime = 0; //для алгоритма работы цикла loop() 10р/с long deltaTime = 0; //для алгоритма работы цикла loop() 10р/с char passRfCtrl[4] = {'*', '*', '*', '*'}; //кодовое слово для проверки приходящего сообщения char act = '?'; //текущая команда int flag_stop_motor = 0;//filtr long time_no_command;//filtr unsigned int impulseTimeL=0; unsigned int distance_smL=0; unsigned int impulseTimeR=0; unsigned int distance_smR=0; void setup() { pinMode(Motor1Dvig1, OUTPUT); pinMode(Motor1Dvig2, OUTPUT); pinMode(Motor2Dvig1, OUTPUT); pinMode(Motor2Dvig2, OUTPUT); pinMode(KsenonL, OUTPUT); pinMode(KsenonR, OUTPUT); pinMode(RegSpeed1, OUTPUT); pinMode(RegSpeed2, OUTPUT); digitalWrite(KsenonL, LOW); digitalWrite(KsenonR, LOW); pinMode(TrigL, OUTPUT); //инициируем как выход pinMode(EchoL, INPUT); //инициируем как вход pinMode(TrigR, OUTPUT); //инициируем как выход pinMode(EchoR, INPUT); //инициируем как вход Serial.begin(9600); vw_set_rx_pin(6); vw_set_ptt_inverted(true); // Required for DR3100 vw_setup(2000); // Bits per sec vw_rx_start(); } //Поворот влево на месте======== void turnL() { digitalWrite(KsenonL, HIGH); digitalWrite(Motor1Dvig1, HIGH); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, HIGH); digitalWrite(Motor2Dvig2, LOW); analogWrite(RegSpeed1, 120); analogWrite(RegSpeed2, 120); } //Поворот вправо на месте======== void turnR() { digitalWrite(KsenonR, HIGH); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, HIGH); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, HIGH); analogWrite(RegSpeed1, 120); analogWrite(RegSpeed2, 120); } //Вперед========================= void goF() { digitalWrite(KsenonR, HIGH); digitalWrite(KsenonL, HIGH); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, HIGH); digitalWrite(Motor2Dvig1, HIGH); digitalWrite(Motor2Dvig2, LOW); for(i=0; i <= 255; i++){ analogWrite(RegSpeed1, i); analogWrite(RegSpeed2, i);} } //Назад=========================== void goB() { digitalWrite(Motor1Dvig1, HIGH); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, HIGH); for(i=0; i <= 255; i++){ analogWrite(RegSpeed1, i); analogWrite(RegSpeed2, i);} } //Стоим и курим)))) void turnS() { analogWrite(RegSpeed1, 0); analogWrite(RegSpeed2, 0); digitalWrite(KsenonR, LOW); digitalWrite(KsenonL, LOW); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, LOW); } //Авто движение по дальномерам. W уже была в проге, добалю описание функции 25.08 void roboT() { digitalWrite(TrigL, HIGH); delayMicroseconds(10); // равный 10 микросекундам digitalWrite(TrigL, LOW); // Отключаем impulseTimeL=pulseIn(EchoL, HIGH); // Замеряем длину импульса distance_smL=impulseTimeL/58; // Пересчитываем в сантиметры digitalWrite( TrigR, HIGH); delayMicroseconds(10); // равный 10 микросекундам digitalWrite(TrigR, LOW); // Отключаем impulseTimeR=pulseIn(EchoR, HIGH); // Замеряем длину импульса distance_smR=impulseTimeR/58; // Пересчитываем в сантиметры if (distance_smL<5) // Если расстояние менее 10 сантиметром { digitalWrite(KsenonR, HIGH); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, HIGH); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, HIGH); analogWrite(RegSpeed1, 100); analogWrite(RegSpeed2, 100); } else if(distance_smR<5) // Если расстояние менее 10 сантиметром { digitalWrite(KsenonL, HIGH); digitalWrite(Motor1Dvig1, HIGH); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, HIGH); digitalWrite(Motor2Dvig2, LOW); analogWrite(RegSpeed1, 100); analogWrite(RegSpeed2, 100); } else if (distance_smL<5 && distance_smR<5) { digitalWrite(Motor1Dvig1, HIGH); digitalWrite(Motor1Dvig2, LOW); digitalWrite(Motor2Dvig1, LOW); digitalWrite(Motor2Dvig2, HIGH); for(i=0; i <= 150; i++){ analogWrite(RegSpeed1, i); analogWrite(RegSpeed2, i);} } else { digitalWrite(KsenonR, HIGH); digitalWrite(KsenonL, HIGH); digitalWrite(Motor1Dvig1, LOW); digitalWrite(Motor1Dvig2, HIGH); digitalWrite(Motor2Dvig1, HIGH); digitalWrite(Motor2Dvig2, LOW); for(i=0; i <= 150; i++){ analogWrite(RegSpeed1, i); analogWrite(RegSpeed2, i);} } delay(100); } void loop() { prevTime = millis(); uint8_t buf[VW_MAX_MESSAGE_LEN]; uint8_t buflen = VW_MAX_MESSAGE_LEN; //----------------------------------------------------- RF control if (vw_get_message(buf, &buflen)) // Non-blocking { time_no_command = millis(); //filtr int i; char letter; for (i = 0; i < buflen-1; i++) { letter = buf[i]; } letter = buf[buflen-1]; Serial.println(letter); switch (letter) { case 'S': act = 'S'; break; case 'B': act = 'B'; break; case 'F': act = 'F'; break; case 'R': act = 'R'; break; case 'L': act = 'L'; break; case 'W': act = 'W'; break; } } switch (act) { case 'S': turnS(); break; case 'B': goF(); break; case 'F': goB(); break; case 'R': turnR(); break; case 'L': turnL(); break; case 'W': roboT(); break; } if(flag_stop_motor == 0 & (millis() - time_no_command) > 1000)//filtr { flag_stop_motor = 1;//filtr turnS();}//filtr else { flag_stop_motor = 0;//filtr } // --------------------------------------------------- Final Section } [/code]Собственно, прошу помощи.
Не совсем понятно из описания в чем проблема.
Беглый просмотр кода-
при посылке команд между пакетами нужно выдерживать паузу, нет кода передачи и приема, желательно выложить
if(flag_stop_motor == 0 & (millis() - time_no_command) > 1000)---- а здесь засада, лучше ставить скобки, можно забыть про приоритеты операций. А команда& это ПОБИТОВОЕ И. Наверно нужно логическое.Проблема в том, что при движении вперед или назад происходит зависание и робот продолжает ехать, не смотря на отпущенный джойстик. В связи с малым багажом знаний, прошу подсказать в какую строчку нужно добавить проверку, приходила ли команда.
Самостоятельно мне не справится.
Ну или, если у кого есть желание вникнуть, посмотреть - может это вовсе не помехи, а криволапый код.