Два дальномера

Лёха22
Offline
Зарегистрирован: 25.01.2015

Добрый день. Начинаю строить робота, столкнулся с проблемой. Стоит 2 дальномера и драйвер двигателя L298N. Один дальномер срабатывает верно(совершает движение и загорается светодиод), другой как-то не правильно - светодиод загорается, а движения нет. Подскажите, пожалуйста, как решить проблему? Правильно работающий R.

int Motor1Dvig1 = 47;
  int Motor1Dvig2 = 49;
  int Motor2Dvig1 = 51;
  int Motor2Dvig2 = 53;
  int RegSpeed1 = 9;
  int RegSpeed2 = 8;
  int Ksenon1 = 4;
  int Ksenon2 = 5;
  int EchoL = 24;
  int EchoR = 28;
  int TrigL = 26;
  int TrigR = 30;
  
  unsigned int impulseTimeL=0; 
  unsigned int distance_smL=0; 
   
  unsigned int impulseTimeR=0; 
  unsigned int distance_smR=0; 
   
  
void setup() {
  // put your setup code here, to run once:
  pinMode(Motor1Dvig1, OUTPUT);
  pinMode(Motor1Dvig2, OUTPUT);
  pinMode(Motor2Dvig1, OUTPUT);
  pinMode(Motor2Dvig2, OUTPUT);
  pinMode(Ksenon1, OUTPUT);
  pinMode(Ksenon2, OUTPUT);
  pinMode(RegSpeed1, OUTPUT);
  pinMode(RegSpeed2, OUTPUT);
  pinMode(TrigL, OUTPUT); //инициируем как выход 
  pinMode(EchoL, INPUT); //инициируем как вход 
  pinMode(TrigR, OUTPUT); //инициируем как выход 
  pinMode(EchoR, INPUT); //инициируем как вход 
  
  digitalWrite(Ksenon1, HIGH);
  digitalWrite(Ksenon2, HIGH);
   
    
}

void loop() {
  
   digitalWrite(TrigL, HIGH);
   delayMicroseconds(10); // равный 10 микросекундам 
   digitalWrite(TrigL, LOW); // Отключаем
  
   impulseTimeL=pulseIn(EchoL, HIGH); // Замеряем длину импульса
   distance_smL=impulseTimeL/58; // Пересчитываем в сантиметры 
   
 if (distance_smL<10) // Если расстояние менее 10 сантиметром 
  {
  
    
  digitalWrite(Ksenon1, HIGH);
  
  digitalWrite(Motor1Dvig1, HIGH);
  digitalWrite(Motor1Dvig2, LOW);

  digitalWrite(Motor2Dvig1, LOW);
  digitalWrite(Motor2Dvig2, HIGH);

  analogWrite(RegSpeed1, 180);
  analogWrite(RegSpeed2, 250);
      
  


  }  
  else 
  {   
    
  digitalWrite(Ksenon1, LOW);   
  analogWrite(RegSpeed1, 0);
  analogWrite(RegSpeed2, 0);
   
  }   
  
  digitalWrite( TrigR, HIGH);
  delayMicroseconds(10); // равный 10 микросекундам 
  digitalWrite(TrigR, LOW); // Отключаем
  
   
   
 
  impulseTimeR=pulseIn(EchoR, HIGH); // Замеряем длину импульса
  distance_smR=impulseTimeR/58; // Пересчитываем в сантиметры 
   
  if (distance_smR<10) // Если расстояние менее 10 сантиметром 
  {
    
    
    
  digitalWrite(Ksenon2, HIGH);
   
  digitalWrite(Motor1Dvig1, HIGH);
  digitalWrite(Motor1Dvig2, LOW);

  digitalWrite(Motor2Dvig1, LOW);
  digitalWrite(Motor2Dvig2, HIGH);

  analogWrite(RegSpeed1, 250);
  analogWrite(RegSpeed2, 180);
   
  }  
  else 
  {  
    
  digitalWrite(Ksenon2, LOW);
    
  analogWrite(RegSpeed1, 0);
  analogWrite(RegSpeed2, 0);
   
  }   
  
  delay(100); 
  
  

  
  

}

 

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

строки #90....112 всегда маскируют действия в строках #52....75

.....пересмотрите свои ИФЫ

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

два замера - потом анализ и действия, а не по очереди - замер-анализ, замер-анализ

Лёха22
Offline
Зарегистрирован: 25.01.2015

Спасибо за ответ.

Поменял - все равно то же самое.

Если убрать действия правого дальномера-левый начинает работать как надо.

SU-27-16 пишет:

строки #90....112 всегда маскируют действия в строках #52....75

.....пересмотрите свои ИФЫ

Не совсем понял..

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

луп выполняется по кругу, крайние действия в нём "затирают" действия прописанные выше крайних.....

разрулите ифами 4 состояния системы после 2-ух замеров лев и прав бортом 

1 - лев > 10 см, прав > 10 см - едем прямо

2 - лев > 10 см, прав < 10 см - объезд левее

3 - лев < 10 см, прав > 10 см - объезд правее

4 - лев < 10 см, прав < 10 см - сдать назад, подвернуть правее/левее, снова прямо

 

два замера, потом 4 ИФА !

Лёха22
Offline
Зарегистрирован: 25.01.2015

Работает только правый

  int Motor1Dvig1 = 47;
  int Motor1Dvig2 = 49;
  int Motor2Dvig1 = 51;
  int Motor2Dvig2 = 53;
  int RegSpeed1 = 9;
  int RegSpeed2 = 8;
  int Ksenon1 = 4;
  int Ksenon2 = 5;
  int EchoL = 24;
  int EchoR = 28;
  int TrigL = 26;
  int TrigR = 30;
  
  unsigned int impulseTimeL=0; 
  unsigned int distance_smL=0; 
   
  unsigned int impulseTimeR=0; 
  unsigned int distance_smR=0; 
   
  
void setup() {
  // put your setup code here, to run once:
  pinMode(Motor1Dvig1, OUTPUT);
  pinMode(Motor1Dvig2, OUTPUT);
  pinMode(Motor2Dvig1, OUTPUT);
  pinMode(Motor2Dvig2, OUTPUT);
  pinMode(Ksenon1, OUTPUT);
  pinMode(Ksenon2, OUTPUT);
  pinMode(RegSpeed1, OUTPUT);
  pinMode(RegSpeed2, OUTPUT);
  pinMode(TrigL, OUTPUT); //инициируем как выход 
  pinMode(EchoL, INPUT); //инициируем как вход 
  pinMode(TrigR, OUTPUT); //инициируем как выход 
  pinMode(EchoR, INPUT); //инициируем как вход 
  
  digitalWrite(Ksenon1, HIGH);
  digitalWrite(Ksenon2, HIGH);
   
    
}

void loop() {
  
   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>2&&distance_smR>2) // Если расстояние менее 10 сантиметром 
  {
  
    
  digitalWrite(Ksenon1, HIGH);
  
  digitalWrite(Motor1Dvig1, HIGH);
  digitalWrite(Motor1Dvig2, LOW);

  digitalWrite(Motor2Dvig1, LOW);
  digitalWrite(Motor2Dvig2, HIGH);

  analogWrite(RegSpeed1, 0);
  analogWrite(RegSpeed2, 0);
      
  


  }  
  
  
  
   
  if (distance_smL<2&&distance_smR>2) // Если расстояние менее 10 сантиметром 
  {
    
    
    
  digitalWrite(Ksenon2, HIGH);
   
  digitalWrite(Motor1Dvig1, HIGH);
  digitalWrite(Motor1Dvig2, LOW);

  digitalWrite(Motor2Dvig1, LOW);
  digitalWrite(Motor2Dvig2, HIGH);

  analogWrite(RegSpeed1, 250);
  analogWrite(RegSpeed2, 150);
   
  }  
 if (distance_smL>2&&distance_smR<2) // Если расстояние менее 10 сантиметром 
  {
    
    
    
  digitalWrite(Ksenon2, HIGH);
   
  digitalWrite(Motor1Dvig1, HIGH);
  digitalWrite(Motor1Dvig2, LOW);

  digitalWrite(Motor2Dvig1, LOW);
  digitalWrite(Motor2Dvig2, HIGH);

  analogWrite(RegSpeed1, 150);
  analogWrite(RegSpeed2, 250);
  
  if (distance_smL<2&&distance_smR<2) // Если расстояние менее 10 сантиметром 
  {
    
    
    
  digitalWrite(Ksenon2, HIGH);
   
  digitalWrite(Motor1Dvig1, HIGH);
  digitalWrite(Motor1Dvig2, LOW);

  digitalWrite(Motor2Dvig1, LOW);
  digitalWrite(Motor2Dvig2, HIGH);

  analogWrite(RegSpeed1, 250);
  analogWrite(RegSpeed2, 250);
   
  }  
 
   
  }  
 
  
  delay(100); 
  
  

  
  

}


 

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

при размещении длинного кода выставляйте ещё галочку "свернуть" :)

код щас гляну.....

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

if (distance_smL>2&&distance_smR>2)

почему && ?

чёбы не запутацца я пишу так 

if ( (distance_smL>2) & (distance_smR>2) )

и, наверное надо & , а не && ? попробуйте 

if ( (distance_smL>2) && (distance_smR>2) )

или

if ( (distance_smL>2) & (distance_smR>2) )

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

строки #1.....12

лучше переделать к виду

int Motor1Dvig1 = 47;       >>>>>>     #define Motor1Dvig1 47

.....это оптимизация кода

Лёха22
Offline
Зарегистрирован: 25.01.2015

Переделал - всё так же(( В изначальном коде, где дальномер зажигал ещё светодиод - дальномер со светодиодом работал, а вот с двигателями какя-то фигня.

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

я не вникал в управление двигателями - мне кажется вы запутались что прописать в каждом из 4-ёх ИФАх - когда что делать и куда, и как крутить движки :(

 

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

строки #58.......71 - если нет препятствия ни справа, ни слева - зачем включать поворотник ? ( про движки пока молчу )

при размещении длинного кода выставляйте ещё галочку "свернуть" :) !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

поворотники обзовите не 1 и 2 , а L и R - так логичнее же ?

так же и моторы  - левый правый, так же и пины левыйВперёд, левый Назад.........................................

Лёха22
Offline
Зарегистрирован: 25.01.2015

Я в данный момент не придаю этому значения - может что - то и неверно в какую сторону крутить, с этим разобраться не сложно. Главное чтобы дальномер заработал как надо. Сейчас платформа (Тамия и редутор тамия) стоит на столе без гусениц-поднес руку к правому дальномеру - колеса вращаются, поднес к левому тишина.

Лёха22
Offline
Зарегистрирован: 25.01.2015

Поворотники остались от старой конструкции с else ) Кода срабатывал дальномер - загорался светодиод. Он как бы не мешает. В следующем коде обязательно поставлю галочку)

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

Лёха22 пишет:

Я в данный момент не придаю этому значения - может что - то и неверно в какую сторону крутить, с этим разобраться не сложно. Главное чтобы дальномер заработал как надо. Сейчас платформа (Тамия и редутор тамия) стоит на столе без гусениц-поднес руку к правому дальномеру - колеса вращаются, поднес к левому тишина.

я вижу картину по другому :(

никаких рук - оба движка крутят

справа помеха - левый движок стоп

слева помеха - правый движок стоп

глухая стена впереди - оба движка стоп

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

начните всё сначала - НО долько дальномеры и поворотники

сделайте код чтобы

- нет препятствий - поворотники не светятся

- слева препятсвие - вкл поворотник R

- справа препятсвие - вкл поворотник L

глухая стена - вкл поворотник R и L

....после этого перейдёте к управлению движками ( отработали одно - перешли к другому )

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

я бы начал с этого 

#define KsenonL 4
#define KsenonR 5
#define EchoL 24
#define EchoR 28
#define TrigL 26
#define TrigR 30
unsigned int impulseTimeL=0; 
unsigned int distance_smL=0; 
unsigned int impulseTimeR=0; 
unsigned int distance_smR=0;
boolean interferenceR;
boolean interferenceL;
//========================================================================
void setup()
{
  pinMode(KsenonL,OUTPUT);
  digitalWrite(KsenonL,LOW);
  pinMode(KsenonR,OUTPUT);
  digitalWrite(KsenonR, LOW);
  pinMode(TrigL,OUTPUT);
  pinMode(EchoL,INPUT);
  pinMode(TrigR,OUTPUT);
  pinMode(EchoR,INPUT);
}
//========================================================================
void loop()
{
  digitalWrite(TrigL,HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigL,LOW);
  impulseTimeL=pulseIn(EchoL,HIGH);
  distance_smL=impulseTimeL/58;
   
  digitalWrite( TrigR,HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigR,LOW);
  impulseTimeR=pulseIn(EchoR,HIGH);
  distance_smR=impulseTimeR/58;

  if(distance_smL<10) interferenceL=1;
    else              interferenceL=0;
    
  if(distance_smR<10) interferenceR=1;
    else              interferenceR=0;

// тут анализируем interferenceL и interferenceR, вкл/выкл поворотники, движки и др.
 
}
//========================================================================
// так проще и понятнее

 

Лёха22
Offline
Зарегистрирован: 25.01.2015

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

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

что мешает спрашивать здесь ? :)

Лёха22
Offline
Зарегистрирован: 25.01.2015

Да в принципе - только название темы)

Лёха22
Offline
Зарегистрирован: 25.01.2015

Как interferenceR присвоить

103	  digitalWrite(Ksenon2, HIGH);
104	    
105	  digitalWrite(Motor1Dvig1, HIGH);
106	  digitalWrite(Motor1Dvig2, LOW);
107	 
108	  digitalWrite(Motor2Dvig1, LOW);
109	  digitalWrite(Motor2Dvig2, HIGH);
110	 
111	  analogWrite(RegSpeed1, 150);
112	  analogWrite(RegSpeed2, 250);

Т.е. вложить в переменную поворот вправо?

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012
#define KsenonL 4
#define KsenonR 5
#define EchoL 24
#define EchoR 28
#define TrigL 26
#define TrigR 30
unsigned int impulseTimeL=0; 
unsigned int distance_smL=0; 
unsigned int impulseTimeR=0; 
unsigned int distance_smR=0;
boolean interferenceR;
boolean interferenceL;
//========================================================================
void setup()
{
  pinMode(KsenonL,OUTPUT);
  digitalWrite(KsenonL,LOW);
  pinMode(KsenonR,OUTPUT);
  digitalWrite(KsenonR, LOW);
  pinMode(TrigL,OUTPUT);
  pinMode(EchoL,INPUT);
  pinMode(TrigR,OUTPUT);
  pinMode(EchoR,INPUT);
}
//========================================================================
void loop()
{
  digitalWrite(TrigL,HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigL,LOW);
  impulseTimeL=pulseIn(EchoL,HIGH);
  distance_smL=impulseTimeL/58;
   
  digitalWrite( TrigR,HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigR,LOW);
  impulseTimeR=pulseIn(EchoR,HIGH);
  distance_smR=impulseTimeR/58;

  if(distance_smL<10) interferenceL=1;
    else              interferenceL=0;
    
  if(distance_smR<10) interferenceR=1;
    else              interferenceR=0;

// тут анализируем interferenceL и interferenceR - это всего лишь переменные-флаги-признаки
// вложить в переменную поворот вправо или другое - нельзя из её определения ( да/нет )
  if(!interferenceL&&!interferenceR)      // нет помех 
  {
    // дальнейшее движение прямо, скорость, например можно увеличить
  }

  if(interferenceL)      // помеха слева
  {
    // действия по включению поворотника, управления движками для поворота правее
  }
 
  if(interferenceR)      // помеха справа
  {
    // действия по включению поворотника, управления движками для поворота левее
  }
 
  if(interferenceL&&interferenceR)      // помеха и слева, и справа
  {
    // действия по включению поворотников, управления движками для отката назад,
    // поворота и дальнейшего движения прямо
  }
}
//========================================================================

 

Лёха22
Offline
Зарегистрирован: 25.01.2015

Я наверное не совсем точно выразился. Это не имело отношение к коду, который Вы мне привели. Я имел ввиду, для наглядности и удобства, в начале программы,действия по повороту вложить в одну строчку, чтобы  вдальнейшем, выполняя поворот не ужно было перечилять кучу операций с двигателями.

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012

чтобы  вдальнейшем, выполняя поворот не ужно было перечилять кучу операций

:) опять не точно..... это называется не переменной - это функция-процедура-подпрограмма :)

если в #21 прописан поворот вправо - то

#define KsenonL 4
#define KsenonR 5
#define EchoL 24
#define EchoR 28
#define TrigL 26
#define TrigR 30
unsigned int impulseTimeL=0; 
unsigned int distance_smL=0; 
unsigned int impulseTimeR=0; 
unsigned int distance_smR=0;
boolean interferenceR;
boolean interferenceL;
//========================================================================
void setup()
{
  pinMode(KsenonL,OUTPUT);
  digitalWrite(KsenonL,LOW);
  pinMode(KsenonR,OUTPUT);
  digitalWrite(KsenonR, LOW);
  pinMode(TrigL,OUTPUT);
  pinMode(EchoL,INPUT);
  pinMode(TrigR,OUTPUT);
  pinMode(EchoR,INPUT);
}
//========================================================================
//=== функция поворота вправо без параметров и возвращаемого значения ====
// можно прописать параметр, например крутизну поворота или 
// скорость в повороте, или время в повороте в секундах
void turnR()
{
  digitalWrite(KsenonR, HIGH);
//  digitalWrite(Motor1Dvig1, HIGH);
//  digitalWrite(Motor1Dvig2, LOW);
//  digitalWrite(Motor2Dvig1, LOW);
//  digitalWrite(Motor2Dvig2, HIGH);
//  analogWrite(RegSpeed1, 150);
//  analogWrite(RegSpeed2, 250);
}
//========================================================================
//=== функция поворота влево без параметров и возвращаемого значения ====
// можно прописать параметр, например крутизну поворота или 
// скорость в повороте, или время в повороте в секундах
void turnL()
{
  digitalWrite(KsenonL, HIGH);
}
//========================================================================
void loop()
{
  digitalWrite(TrigL,HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigL,LOW);
  impulseTimeL=pulseIn(EchoL,HIGH);
  distance_smL=impulseTimeL/58;
   
  digitalWrite( TrigR,HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigR,LOW);
  impulseTimeR=pulseIn(EchoR,HIGH);
  distance_smR=impulseTimeR/58;

  if(distance_smL<10) interferenceL=1;
    else              interferenceL=0;
    
  if(distance_smR<10) interferenceR=1;
    else              interferenceR=0;


  if(interferenceL)      // помеха слева
  {
    turnR();
  }
 
  if(interferenceR)      // помеха справа
  {
    turnL();
  }
 
  if(interferenceL&&interferenceR)      // помеха и слева, и справа
  {
    // действия по включению поворотников, управления движками для отката назад,
    // поворота и дальнейшего движения прямо
  }
}
//========================================================================

смотри строки 25......38 -> 71

                       39......46 -> 76

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012
//=== функция поворота вправо С ПАРАМЕТРАМИ и без возвращаемого значения ====
// два параметра - скорости движков
void turnR(byte speedL,byte speedR)
{
  digitalWrite(KsenonR, HIGH);
  digitalWrite(Motor1Dvig1, HIGH);
  digitalWrite(Motor1Dvig2, LOW);
  digitalWrite(Motor2Dvig1, LOW);
  digitalWrite(Motor2Dvig2, HIGH);
  analogWrite(RegSpeed1, speedL);
  analogWrite(RegSpeed2, speedR);
}
// вызов функции
  turnR(150,250);

 

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012
//=== функция движения С ПАРАМЕТРАМИ и без возвращаемого значения ====
// directionL - движок левый  0-назад, 1-вперёд
// directionR - движок правый 0-назад, 1-вперёд
// speedL, speedR - скорость
void turn(boolean directionL,byte speedL,boolean directionR,byte speedR)
{
//  digitalWrite(KsenonR, HIGH);
  digitalWrite(MotorLDvig1,directionL);
  digitalWrite(MotorLDvig2,!directionL);
  digitalWrite(MotorRDvig1,directionR);
  digitalWrite(MotorRDvig2,!directionR);
  analogWrite(RegSpeedL, speedL);
  analogWrite(RegSpeedR, speedR);
}
// вызов функции
  turn(1,150,1,150);  // прямо медленно
  turn(1,250,1,250);  // прямо полный ход
  turn(1,250,1,150);  // вправо по пологой дуге
  turn(1,250,1,50);   // вправо по крутой дуге  
  turn(1,250,0,250);  // разворот-вращение вправо на месте ( по танковому )
  turn(1,100,0,100);  // разворот-вращение вправо на месте ( по танковому ) помедленнее
  turn(0,100,0,100);  // медленно назад
// одной функцией управляем как надо, как хотим

 

Лёха22
Offline
Зарегистрирован: 25.01.2015

Спасибо большое, буду изучать!

SU-27-16
SU-27-16 аватар
Offline
Зарегистрирован: 13.08.2012