Главная » Форумы » Программирование

Проблема с поботом на uno

11 ответов [Последнее сообщение]
Вс, 18/02/2018 - 11:34
Elmirus
Offline
Зарегистрирован: 07.06.2017

Доброго времени суток. Собрал 2х колесного робота на arduino uno. 4ик датчика препятствий и 2концевых выключателя на бампере подключены к аналоговым входам с А0 по А6, соотаетственно обозначит в коде их как цифровые с 14 по 19 и сконфигурировал их как +5, датчики при контакте дают на пины gnd.
Проблема заключается в том, что датчики с левой стороны с А0 по А2 срабатывают мгновенно при столкновении робота, а вот с правой стороны с А3 по А5 работают с очень большой затормозкой, т.е. робот врезается в стену и только через 3 секунды начинает поворачивать.
Замеряв мультиметром было выяснено что при срабатывании датчика напряжение на пине с +5 до 0 падает очень медленно, секунды 3, в чем и заключается проблема, а оно должно быть мгновенным.

Верх
Вс, 18/02/2018 - 11:53
#1
andriano
andriano аватар
Offline
Зарегистрирован: 20.06.2015

Если Вы хотите похвастаться результатом, для этого есть раздел "Проекты", а если у Вас есть вопрос - их можно задавать в других разделах (hint: вопрос должен заканчиваться знаком "?").

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

Верх
Вс, 18/02/2018 - 13:04
#2
Elmirus
Offline
Зарегистрирован: 07.06.2017

Вот собственно скетч. Хвастаться чем либо у меня нет времени. Я подчеркнул свою поблему в вопросе, на половине аналоговых входах напряжение при включении датчика падает мгновенно, а на второй половине входах падает очень медленно, я считаю, что проблема скорее всего не в скетче, темболее все  датчики подключены одинаково. 

#include <AFMotor.h> // Подключаем библиотеку для управления двигателями 
#include <SoftwareSerial.h> // Подключаем библиотеку для работы с Serial через дискретные порты

// Создаем объекты для датчиков бампера
const int bumperPin1 = 14;
const int bumperPin2 = 15;
const int irdaSensorPin1 = 19;
const int irdaSensorPin2 = 18;
const int irdaSensorPin3 = 17;
const int irdaSensorPin4 = 16;
byte v;
byte b;
byte n;
byte m;
int bumperPinStateLeft = 0;
int bumperPinStateRight = 0;

// Создаем объекты для двигателей 
AF_DCMotor motor1(1); //канал М1 на Motor Shield — турбина 
AF_DCMotor motor2(2); //канал М1 на Motor Shield — веник 
AF_DCMotor motor3(3); //канал М1 на Motor Shield — левый двигатель
AF_DCMotor motor4(4); //канал М2 на Motor Shield — правый двигатель

//
SoftwareSerial BTSerial(2, 13); // RX, TX

// Создаем переменную для команд Bluetooth 
char vcmd; 
// Создаем переменные для запоминания скорости двигателей 
int vspdL, vspdR; 
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах. 
Текущая скорость должна быть больше этого значения. В противном случае двигатели со стороны направления поворота просто не будут вращаться */ 
int vspd = 200;

void setup() { 
  // 
 pinMode(irdaSensorPin1, INPUT);
 pinMode(irdaSensorPin2, INPUT);
 pinMode(irdaSensorPin3, INPUT);
 pinMode(irdaSensorPin4, INPUT);
 pinMode(bumperPin1,INPUT);
 pinMode(bumperPin2,INPUT);

// Устанавливаем скорость передачи данных по Bluetooth 
BTSerial.begin(9600); 
// Устанавливаем скорость передачи данных по кабелю 
Serial.begin(9600);  
// Устанавливаем максимальную скорость вращения двигателей 
vspeed(255,255); 
}

void loop() { 
  uint8_t i;

v = digitalRead( irdaSensorPin1 );
b = digitalRead( irdaSensorPin2 );
n = digitalRead( irdaSensorPin3 );
m = digitalRead( irdaSensorPin4 );
bumperPinStateLeft = digitalRead(bumperPin1);
bumperPinStateRight = digitalRead(bumperPin2);
//============================================

  // Читаем команды и заносим их в переменную. char преобразует код символа команды в символ 
vcmd = (char)BTSerial.read(); 
// Отправляем команду в порт, чтобы можно было их проверить в «Мониторе порта» 
Serial.println(vcmd);

/*
  for (i=0; i<20; i++) {
motor3.run(FORWARD); 
motor4.run(FORWARD); 
    delay(10);
 }
*/

//Бамперы
 if (bumperPinStateLeft == HIGH) {
    vrelease();
    delay(100);
    vbackward();
    delay(300);
    vleft(); // Реверс
    delay(random(400, 900));
    vrelease();
    delay(100);
 } else {

  }

  if (bumperPinStateRight == HIGH) {
    vrelease();
    delay(100);
    vbackward();
    delay(300);
    vright(); // Реверс
    delay(random(400, 900));
    vrelease();
    delay(100);
    } else {
 
  }


//ИК датчики
 if( v == LOW ) {
    vrelease();
    delay(100);
    vright();
    delay(200);
    vrelease();
 }
  //vbackward();
 if( b == LOW ) {
    vrelease();
    delay(100);
    vright();
    delay(600);
    vrelease();
 }

  if( n == LOW ) {
    vrelease();
    delay(100);
    vleft();
    delay(600);
    vrelease();
  }

  if( m == LOW ) {
    vrelease();
    delay(100);
    vleft();
    delay(200);
    vrelease();
  }
//============================================


// Если есть данные 
if (BTSerial.available()) 
{

//================== Мои функции=============
//Бамперы
 if (bumperPinStateLeft == HIGH) {
    vrelease();
    delay(100);
    vbackward();
    delay(200);
    vleft(); // Реверс
    delay(random(400, 900));
    vrelease();
    delay(100);
 } else {

  }

  if (bumperPinStateRight == HIGH) {
    vrelease();
    delay(1000);
    vbackward();
    delay(200);
    vright(); // Реверс
    delay(random(400, 900));
    vrelease();
    delay(100);
    } else {
 
  }

//ИК датчики  
 if( v == LOW ) {
    vrelease();
    delay(100);
    vright();
    delay(200);
    vrelease();
 }
  //vbackward();
 if( b == LOW ) {
    vrelease();
    delay(100);
    vright();
    delay(600);
    vrelease();
 }

  if( n == LOW ) {
    vrelease();
    delay(100);
    vleft();
    delay(600);
    vrelease();
  }

  if( m == LOW ) {
    vrelease();
    delay(100);
    vleft();
    delay(200);
    vrelease();
  }
//============================================

// Вперед 
if (vcmd == 'F') { 
vforward(); 
} 
// Назад 
if (vcmd == 'B') 
{ 
vbackward(); 
} 
// Влево 
if (vcmd == 'L') 
{ 
vleft(); 
} 
// Вправо 
if (vcmd == 'R') 
{ 
vright(); 
} 
// Прямо и влево 
if (vcmd == 'G') 
{ 
vforwardleft(); 
} 
// Прямо и вправо 
if (vcmd == 'I') 
{ 
vforwardright(); 
} 
// Назад и влево 
if (vcmd == 'H') 
{ 
vbackwardleft(); 
} 
// Назад и вправо 
if (vcmd == 'J') 
{ 
vbackwardright(); 
} 
// Стоп 
if (vcmd == 'S') 
{ 
vrelease(); 
} 
// Скорость 0% 
if (vcmd == '0') 
{ 
vspeed(0,0); 
} 
// Скорость 10% 
if (vcmd == '1') 
{ 
vspeed(25,25); 
} 
// Скорость 20% 
if (vcmd == '2') 
{ 
vspeed(50,50); 
} 
// Скорость 30% 
if (vcmd == '3') 
{ 
vspeed(75,75); 
} 
// Скорость 40% 
if (vcmd == '4') 
{ 
vspeed(100,100); 
} 
// Скорость 50% 
if (vcmd == '5') 
{ 
vspeed(125,125); 
} 
// Скорость 60% 
if (vcmd == '6') 
{ 
vspeed(150,150); 
} 
// Скорость 70% 
if (vcmd == '7') 
{ 
vspeed(175,175); 
} 
// Скорость 80% 
if (vcmd == '8') 
{ 
vspeed(200,200); 
} 
// Скорость 90% 
if (vcmd == '9') 
{ 
vspeed(225,225); 
} 
// Скорость 100% 
if (vcmd == 'q') 
{ 
vspeed(255,255); 
} 
} 
}








//================== Мои функции === Автостарт============



//========================================================= 


// Вперед 
void vforward() { 
vspeed(vspdL,vspdR); 
vforwardRL(); 
}

// Вперед для RL 
void vforwardRL() { 
motor3.run(FORWARD); 
motor4.run(FORWARD); 
}

// Назад 
void vbackward() { 
vspeed(vspdL,vspdR); 
vbackwardRL(); 
}

// Назад для RL 
void vbackwardRL() { 
motor3.run(BACKWARD); 
motor4.run(BACKWARD); 
}

// Влево 
void vleft() { 
vspeed(vspdL,vspdR); 
motor3.run(FORWARD); 
motor4.run(BACKWARD); 
}

// Вправо 
void vright() { 
vspeed(vspdL,vspdR); 
motor3.run(BACKWARD); 
motor4.run(FORWARD); 
}

// Вперед и влево 
void vforwardleft() { 
if (vspdL > vspd) { 
vspeed(vspdL-vspd,vspdR); 
} 
else 
{ 
vspeed(0,vspdR); 
} 
vforwardRL(); 
}

// Вперед и вправо 
void vforwardright() { 
if (vspdR > vspd) { 
vspeed(vspdL,vspdR-vspd); 
} 
else 
{ 
vspeed(vspdL,0); 
} 
vforwardRL(); 
}

// Назад и влево 
void vbackwardleft() { 
if (vspdL > vspd) { 
vspeed(vspdL-vspd,vspdR); 
} 
else 
{ 
vspeed(0,vspdR); 
} 
vbackwardRL(); 
}

// Назад и вправо 
void vbackwardright() { 
if (vspdR > vspd) { 
vspeed(vspdL,vspdR-vspd); 
} 
else 
{ 
vspeed(vspdL,0); 
} 
vbackwardRL(); 
}

// Стоп 
void vrelease(){ 
motor3.run(RELEASE); 
motor4.run(RELEASE); 
}

// Изменение скорости 
void vspeed(int spdL,int spdR){ 
if (spdL == spdR) { 
vspdL=spdL; 
vspdR=spdR; 
} 
motor3.setSpeed(spdL); 
motor4.setSpeed(spdR); 
}

Верх
Вс, 18/02/2018 - 13:26
#3
b707
Offline
Зарегистрирован: 26.05.2017

Elmirus, где вы взяли этот ужасный код? Вы явно в нем ничего не понимаете, просто тупо скопировали из инета. Почему я так думаю? - да потому что если б вы хоть чуть понимали - заметили бы, что часть кода повторяется в скетче дважды - с 76 строки и 144.

Код этот  работать не будет, его даже исправлять нет смысла - нужно просто выкинуть и все. Как может робот обьезжать препятския, если в функциях реакции на датчики встроены задержки до секунды длиной?!

Ну и еще куча мелких ляпов, опять же показывающих, что вы ничего не понимаете в программировании.

- зачем к IF в строках 77 и 90 приделан пустой ELSE?

- почему вы сначала читаете из сериала (строка 64) и только потом проверяете(строка 140), если ли там что-нибудь? :)

 

Но главное - все функции движения надо переписать без задержек.

-

Верх
Вс, 18/02/2018 - 13:40
#4
Elmirus
Offline
Зарегистрирован: 07.06.2017

Так и есть я скопировал в интернете. 2 раза повторяется потому как 2й раз этиже функции работают при подключенном блютузе. С функциями до конца не разобрался и потому такие повторы. Без пустого else чегото при однократном нажатии моторы крутятся безконечно. Только сделал пустой else сразу начало нормально работать. Непойму чего так, ведь ик датчики работают по тому же принципу и с ними все нормально.

Верх
Вс, 18/02/2018 - 13:46
#5
b707
Offline
Зарегистрирован: 26.05.2017

Elmirus пишет:
2 раза повторяется потому как 2й раз этиже функции работают при подключенном блютузе. С функциями до конца не разобрался и потому такие повторы. Без пустого else чегото при однократном нажатии моторы крутятся безконечно. Только сделал пустой else сразу начало нормально работать. .

Простите, но процитированное - откровенный бред.

По скетчу - советую проблемы робота пока оставить и начать с изучения основ. Прочитайте раздел "Программирование" на этом сайте. Помигайте диодом. Потом прочитайте материал "Блинк бех делей" и попробуйте переписать свой скетч без задержек.

Верх
Вс, 18/02/2018 - 14:00
#6
b707
Offline
Зарегистрирован: 26.05.2017

я вам написал

прочитайте материал "Блинк бех делей"

 

Верх
Втр, 20/02/2018 - 09:49
#7
Elmirus
Offline
Зарегистрирован: 07.06.2017

Я попробовал сделать так, но не могу понять почему при каждом срабатывании время отъезда назад разное? Первое срабатывание павильно по заданному времени, а вот последующие длинее. Походу, я безнадежен(


    if (bumperPinStateRight == HIGH)
 {
  previousMillis = millis(); // запоминаем момент времени
  vbackward();
  if (millis() - previousMillis >= 100){
    vrelease();
    previousMillis = 0;
  }
 }

 

 

Верх
Чт, 22/02/2018 - 20:23
#8
Elmirus
Offline
Зарегистрирован: 07.06.2017

Доброго времени суток. Вот сделал все без deley, еле еле немного переварил этот millis. Все работает отлично! Однако аппаратная проблема осталась без изменения. Прошу вас о помощи, почему так случается, что левый бампер для сработки блока кода в if надо зажимать мгновенно, а вот правый бампер надо жать секунды 3 и только потом робот начнет реагировать?

 

Код прилагаю:

#include <AFMotor.h> // Подключаем библиотеку для управления двигателями 

unsigned long bumperRightWait;
bool bumper1ON;
unsigned long waitTime1;
bool waitTime1ON;

unsigned long bumperLeftWait;
bool bumper2ON;
unsigned long waitTime2;
bool waitTime2ON;


// Создаем объекты для датчиков бампера
const int bumperPin1 = 14;
const int bumperPin2 = 15;
const int irdaSensorPin1 = 19;
const int irdaSensorPin2 = 18;
const int irdaSensorPin3 = 17;
const int irdaSensorPin4 = 16;
byte v;
byte b;
byte n;
byte m;
int bumperPinStateLeft = 0;
int bumperPinStateRight = 0;

// Создаем объекты для двигателей 
AF_DCMotor motor3(3); //канал М1 на Motor Shield — левый двигатель
AF_DCMotor motor4(4); //канал М2 на Motor Shield — правый двигатель 

void setup() {
  // Конфигурация бампера
 pinMode(irdaSensorPin1, INPUT);
 pinMode(irdaSensorPin2, INPUT);
 pinMode(irdaSensorPin3, INPUT);
 pinMode(irdaSensorPin4, INPUT);
 pinMode(bumperPin1,INPUT);
 pinMode(bumperPin2,INPUT);
 Serial.begin(9600); 

bumper1ON = false;
waitTime1ON = false;
waitTime2ON = false;

bumper2ON = false;
}


void loop() {
uint8_t i;
v = digitalRead( irdaSensorPin1 );
b = digitalRead( irdaSensorPin2 );
n = digitalRead( irdaSensorPin3 );
m = digitalRead( irdaSensorPin4 );
bumperPinStateLeft = digitalRead(bumperPin1);
bumperPinStateRight = digitalRead(bumperPin2);



//Бамперы ==================================================================
if (bumperPinStateRight == HIGH) {vbackward(); bumper1ON = true; bumperRightWait = millis();} // Левый
if (bumperPinStateLeft == HIGH) {vbackward(); bumper2ON = true; bumperLeftWait = millis();}   //Правый

//ЛЕВЫЙ===========================================================
if(bumper1ON) {
if(millis() - bumperRightWait > 300) {
  vrelease(); 
  bumper1ON = false;
  vleft(); waitTime1ON = true; waitTime1 = millis();
  }
}

if(waitTime1ON) {
if(millis() - waitTime1 > 600) {
  vrelease(); 
  waitTime1ON = false;
  }
}

//ПРАВЫЙ==================================================
if(bumper2ON) {
if(millis() - bumperLeftWait > 300) {
  vrelease(); 
  bumper2ON = false;
  vright(); waitTime2ON = true; waitTime2 = millis();
  }
}

if(waitTime2ON) {
if(millis() - waitTime2 > 600) {
  vrelease(); 
  waitTime2ON = false;
  }
}

/*
    //ИК датчики
 if( v == LOW ) {
    vrelease();
    delay(100);
    vright();
    delay(200);
    vrelease();
 }
  //vbackward();
 if( b == LOW ) {
    vrelease();
    delay(100);
    vright();
    delay(600);
    vrelease();
 }

  if( n == LOW ) {
    vrelease();
    delay(100);
    vleft();
    delay(600);
    vrelease();
  }

  if( m == LOW ) {
    vrelease();
    delay(100);
    vleft();
    delay(200);
    vrelease();
  }
*/
}


// FUNCTION ===============================================================

// Влево 
void vleft() {  
motor3.run(FORWARD); 
motor4.run(BACKWARD);
motor3.setSpeed(255);
motor4.setSpeed(255); 
}

// Вправо 
void vright() { 
motor3.run(BACKWARD); 
motor4.run(FORWARD);
motor3.setSpeed(255);
motor4.setSpeed(255); 
}

// Стоп 
void vrelease(){ 
motor3.run(RELEASE); 
motor4.run(RELEASE);
}

// Вперед  
void vforward() { 
motor3.run(FORWARD); 
motor4.run(FORWARD); 
motor3.setSpeed(255);
motor4.setSpeed(255);
}

// Назад 
void vbackward() { 
motor3.run(BACKWARD); 
motor4.run(BACKWARD);
motor3.setSpeed(255);
motor4.setSpeed(255); 
}

 

Верх
Пт, 23/02/2018 - 23:24
#9
Elmirus
Offline
Зарегистрирован: 07.06.2017

Помогите! Аналоговые входыс А0 по А4 работают отлично но А5 и А6 тормозят! Что делать?

#include <AFMotor.h>

unsigned long bumperRightWait; bool bumper1ON;
unsigned long waitTime1; bool waitTime1ON;

unsigned long bumperLeftWait; bool bumper2ON;
unsigned long waitTime2; bool waitTime2ON;

unsigned long IRDA1; bool IRDAON1;
unsigned long IRDA2; bool IRDAON2;
unsigned long IRDA3; bool IRDAON3;
unsigned long IRDA4; bool IRDAON4;

const int bumperPin1 = 14;
const int bumperPin2 = 15;
const int irdaSensorPin1 = 19;
const int irdaSensorPin2 = 18;
const int irdaSensorPin3 = 17;
const int irdaSensorPin4 = 16;
byte v; byte b; byte n; byte m;
int bumperPinStateLeft = 0;
int bumperPinStateRight = 0;

AF_DCMotor motor3(3); AF_DCMotor motor4(4);

void setup() {
 pinMode(irdaSensorPin1, INPUT);
 pinMode(irdaSensorPin2, INPUT);
 pinMode(irdaSensorPin3, INPUT);
 pinMode(irdaSensorPin4, INPUT);
 pinMode(bumperPin1,INPUT);
 pinMode(bumperPin2,INPUT);
 Serial.begin(9600); 

bumper1ON = false; waitTime1ON = false;
bumper2ON = false; waitTime2ON = false;
IRDAON1 = false; IRDAON2 = false; 
IRDAON3 = false; IRDAON4 = false; 
}


void loop() {
uint8_t i;
v = digitalRead( irdaSensorPin1 );
b = digitalRead( irdaSensorPin2 );
n = digitalRead( irdaSensorPin3 );
m = digitalRead( irdaSensorPin4 );
bumperPinStateLeft = digitalRead(bumperPin1);
bumperPinStateRight = digitalRead(bumperPin2);

//vforward();

//Бамперы ===============================================================================================
if (bumperPinStateRight == HIGH) {vbackward(); bumper1ON = true; bumperRightWait = millis();} // Левый
if (bumperPinStateLeft == HIGH) {vbackward(); bumper2ON = true; bumperLeftWait = millis();}   //Правый

//ЛЕВЫЙ===========================================================
if(bumper1ON) {
if(millis() - bumperRightWait > 300) {vrelease(); bumper1ON = false;
  vleft(); waitTime1ON = true; waitTime1 = millis();
  }
}
if(waitTime1ON) {if(millis() - waitTime1 > 700) {vrelease(); waitTime1ON = false;}}

//ПРАВЫЙ=========================================================
if(bumper2ON) {
if(millis() - bumperLeftWait > 300) {vrelease(); bumper2ON = false;
  vright(); waitTime2ON = true; waitTime2 = millis();
  }
}
if(waitTime2ON) {if(millis() - waitTime2 > 700) {vrelease(); waitTime2ON = false; }}

 //ИК датчики=================================================================================================
if (v == LOW) {vleft(); IRDAON1 = true; IRDA1 = millis();}   //1
if (b == LOW) {vleft(); IRDAON2 = true; IRDA2 = millis();}   //2
if (n == LOW) {vright(); IRDAON3 = true; IRDA3 = millis();}   //3
if (m == LOW) {vright(); IRDAON4 = true; IRDA4 = millis();}   //4

if(IRDAON1) {if(millis() - IRDA1 > 300) {vrelease(); IRDAON1 = false;}}
if(IRDAON2) {if(millis() - IRDA2 > 600) {vrelease(); IRDAON2 = false;}}
if(IRDAON3) {if(millis() - IRDA3 > 600) {vrelease(); IRDAON3 = false;}}
if(IRDAON4) {if(millis() - IRDA4 > 300) {vrelease(); IRDAON4 = false;}}
}

// Функции ===============================================================

// Влево 
void vleft() {motor3.run(FORWARD); motor4.run(BACKWARD); 
motor3.setSpeed(255); motor4.setSpeed(255); }

// Вправо 
void vright() {motor3.run(BACKWARD); motor4.run(FORWARD); 
motor3.setSpeed(255); motor4.setSpeed(255); }

// Стоп 
void vrelease(){motor3.run(RELEASE); 
motor4.run(RELEASE); }

// Вперед  
void vforward() {motor3.run(FORWARD); motor4.run(FORWARD); 
motor3.setSpeed(255); motor4.setSpeed(255);}

// Назад 
void vbackward() {motor3.run(BACKWARD); motor4.run(BACKWARD); 
motor3.setSpeed(255); motor4.setSpeed(255);}

 

Верх
Вс, 25/02/2018 - 02:41
#10
Elmirus
Offline
Зарегистрирован: 07.06.2017

Код без единного delay

#include <AFMotor.h>
#include <SoftwareSerial.h> 
SoftwareSerial BTSerial(2, 13); // RX, TX

unsigned long allSensor; bool allSensorON; unsigned long allSensor1; bool allSensor1ON;
unsigned long bumperRightWait; bool bumper1ON; unsigned long waitTime1; bool waitTime1ON;
unsigned long bumperLeftWait; bool bumper2ON; unsigned long waitTime2; bool waitTime2ON;
unsigned long IRDA1; bool IRDAON1; unsigned long IRDA2; bool IRDAON2;
unsigned long IRDA3; bool IRDAON3; unsigned long IRDA4; bool IRDAON4;
bool forwardON; bool turbON; bool venikON;

const int bumperPin1 = 14;
const int bumperPin2 = 15;
const int irdaSensorPin1 = 19;
const int irdaSensorPin2 = 18;
const int irdaSensorPin3 = 17;
const int irdaSensorPin4 = 16;
byte v; byte b; byte n; byte m;
int bumperPinStateLeft = 0;
int bumperPinStateRight = 0;
char vcmd;
int mSpeed1 = 225; //Моторы
int mSpeed2 = 200; //Движение вперед
int mSpeed3 = 255; //Турбина
int mSpeed4 = 255; //Веник

AF_DCMotor motor3(3); AF_DCMotor motor4(4); AF_DCMotor motor1(1); AF_DCMotor motor2(2);

void setup() {
 pinMode(irdaSensorPin1, INPUT);
 pinMode(irdaSensorPin2, INPUT);
 pinMode(irdaSensorPin3, INPUT);
 pinMode(irdaSensorPin4, INPUT);
 pinMode(bumperPin1,INPUT);
 pinMode(bumperPin2,INPUT);
 Serial.begin(9600); BTSerial.begin(9600);

allSensorON = false; allSensor1ON = false; 
bumper1ON = false; waitTime1ON = false;
bumper2ON = false; waitTime2ON = false;
IRDAON1 = false; IRDAON2 = false; 
IRDAON3 = false; IRDAON4 = false; 
forwardON = true; turbON = true; venikON = true;
}


void loop() {
uint8_t i;
v = digitalRead( irdaSensorPin1 );
b = digitalRead( irdaSensorPin2 );
n = digitalRead( irdaSensorPin3 );
m = digitalRead( irdaSensorPin4 );
bumperPinStateLeft = digitalRead(bumperPin1);
bumperPinStateRight = digitalRead(bumperPin2);
int randNumber = random(600, 1000);

if (forwardON){vforward();} //
if (turbON) {motor1.run(BACKWARD); motor1.run(FORWARD); motor1.setSpeed(mSpeed3);} //
if (venikON) {motor1.run(BACKWARD); motor1.run(FORWARD); motor1.setSpeed(mSpeed4);} //

if (v == LOW && b == LOW && bumperPinStateRight == HIGH || v == LOW && b == LOW 
|| b == LOW && bumperPinStateRight == HIGH || v == LOW && bumperPinStateRight == HIGH)
{forwardON = false; vbackward(); bumper1ON = true; bumperRightWait = millis();}
if (n == LOW && m == LOW && bumperPinStateLeft == HIGH || n == LOW && b == LOW 
|| m == LOW && bumperPinStateLeft == HIGH || n == LOW && bumperPinStateLeft == HIGH)
{forwardON = false; vbackward(); bumper2ON = true; bumperLeftWait = millis();}
if (bumperPinStateRight == HIGH && bumperPinStateLeft == HIGH || v == LOW && m == LOW 
|| b == LOW && n == LOW || v == LOW && n == LOW || b == LOW && m == LOW)
{forwardON = false; vbackward(); allSensorON = true; allSensor = millis();}

if(allSensorON) {
if(millis() - allSensor > 600) {vrelease(); allSensorON = false;
  vleft(); allSensor1ON = true; allSensor1 = millis();
  }
}
if(allSensor1ON) {if(millis() - allSensor1 > 1400) {vrelease(); allSensor1ON = false; forwardON = true;}}

//Бамперы ===============================================================================================
if (bumperPinStateRight == HIGH) {forwardON = false; vbackward(); bumper1ON = true; bumperRightWait = millis();} // Левый
if (bumperPinStateLeft == HIGH) {forwardON = false; vbackward(); bumper2ON = true; bumperLeftWait = millis();}   //Правый

//ЛЕВЫЙ===========================================================
if(bumper1ON) {
if(millis() - bumperRightWait > 300) {vrelease(); bumper1ON = false;
  vleft(); waitTime1ON = true; waitTime1 = millis();
  }
}
if(waitTime1ON) {if(millis() - waitTime1 > randNumber) {vrelease(); waitTime1ON = false; forwardON = true;}}

//ПРАВЫЙ=========================================================
if(bumper2ON) {
if(millis() - bumperLeftWait > 300) {vrelease(); bumper2ON = false;
  vright(); waitTime2ON = true; waitTime2 = millis();
  }
}
if(waitTime2ON) {if(millis() - waitTime2 > randNumber) {vrelease(); waitTime2ON = false; forwardON = true;}}

 //ИК датчики=================================================================================================
if (v == LOW) {forwardON = false; vleft(); IRDAON1 = true; IRDA1 = millis();}   //1
if (b == LOW) {forwardON = false; vleft(); IRDAON2 = true; IRDA2 = millis();}   //2
if (n == LOW) {forwardON = false; vright(); IRDAON3 = true; IRDA3 = millis();}   //3
if (m == LOW) {forwardON = false; vright(); IRDAON4 = true; IRDA4 = millis();}   //4

if(IRDAON1) {if(millis() - IRDA1 > 300) {vrelease(); IRDAON1 = false; forwardON = true;}}
if(IRDAON2) {if(millis() - IRDA2 > 600) {vrelease(); IRDAON2 = false; forwardON = true;}}
if(IRDAON3) {if(millis() - IRDA3 > 600) {vrelease(); IRDAON3 = false; forwardON = true;}}
if(IRDAON4) {if(millis() - IRDA4 > 300) {vrelease(); IRDAON4 = false; forwardON = true;}}

vcmd = (char)BTSerial.read(); 
Serial.println(vcmd);

if (BTSerial.available()) 
 {
  forwardON = false;
  if (vcmd == 'F') {vforward();} 
  if (vcmd == 'B') {vbackward();} 
  if (vcmd == 'L') {vleft();} 
  if (vcmd == 'R') {vright();} 
  //if (vcmd == 'G') {vforwardleft();} 
  //if (vcmd == 'I') {vforwardright();} 
  //if (vcmd == 'H') {vbackwardleft();} 
  //if (vcmd == 'J') {vbackwardright();} 
  if (vcmd == 'S') {vrelease();}
  if (vcmd == 'W') {forwardON = true;} 
 }
} // Конец основного цикла

// Функции ===============================================================

// Влево 
void vleft() {motor3.run(FORWARD); motor4.run(BACKWARD); 
motor3.setSpeed(mSpeed1); motor4.setSpeed(mSpeed1); }

// Вправо 
void vright() {motor3.run(BACKWARD); motor4.run(FORWARD); 
motor3.setSpeed(mSpeed1); motor4.setSpeed(mSpeed1); }

// Стоп 
void vrelease(){motor3.run(RELEASE); 
motor4.run(RELEASE); }

// Вперед  
void vforward() {motor3.run(FORWARD); motor4.run(FORWARD); 
motor3.setSpeed(mSpeed2); motor4.setSpeed(mSpeed2);}

// Назад 
void vbackward() {motor3.run(BACKWARD); motor4.run(BACKWARD); 
motor3.setSpeed(mSpeed1); motor4.setSpeed(mSpeed1);}

 

Верх
Пнд, 26/02/2018 - 00:01
#11
Elmirus
Offline
Зарегистрирован: 07.06.2017

Проблема была решена отмачиванием плат в спирте. Конечный код

#include <AFMotor.h>
#include <SoftwareSerial.h> 
SoftwareSerial BTSerial(2, 13); // RX, TX

unsigned long allSensor; bool allSensorON; unsigned long allSensor1; bool allSensor1ON;
unsigned long bumperRightWait; bool bumper1ON; unsigned long waitTime1; bool waitTime1ON;
unsigned long bumperLeftWait; bool bumper2ON; unsigned long waitTime2; bool waitTime2ON;
unsigned long IRDA1; bool IRDAON1; unsigned long IRDA2; bool IRDAON2;
unsigned long IRDA3; bool IRDAON3; unsigned long IRDA4; bool IRDAON4;
bool forwardON; bool turbON; bool venikON;

const int bumperPin1 = 14;
const int bumperPin2 = 15;
const int irdaSensorPin1 = 19;
const int irdaSensorPin2 = 18;
const int irdaSensorPin3 = 17;
const int irdaSensorPin4 = 16;
byte v; byte b; byte n; byte m;
int bumperPinStateLeft = 0;
int bumperPinStateRight = 0;
char vcmd;
int mSpeed1 = 225; //Моторы
int mSpeed2 = 200; //Движение вперед
int mSpeed3 = 255; //Турбина
int mSpeed4 = 255; //Веник

AF_DCMotor motor3(3); AF_DCMotor motor4(4); AF_DCMotor motor1(1); AF_DCMotor motor2(2);

void setup() {
 pinMode(irdaSensorPin1, INPUT);
 pinMode(irdaSensorPin2, INPUT);
 pinMode(irdaSensorPin3, INPUT);
 pinMode(irdaSensorPin4, INPUT);
 pinMode(bumperPin1,INPUT);
 pinMode(bumperPin2,INPUT);
 Serial.begin(9600); BTSerial.begin(9600);

allSensorON = false; allSensor1ON = false; 
bumper1ON = false; waitTime1ON = false;
bumper2ON = false; waitTime2ON = false;
IRDAON1 = false; IRDAON2 = false; 
IRDAON3 = false; IRDAON4 = false; 
forwardON = true; //turbON = true; venikON = true;
}


void loop() {
uint8_t i;
v = digitalRead( irdaSensorPin1 );
b = digitalRead( irdaSensorPin2 );
n = digitalRead( irdaSensorPin3 );
m = digitalRead( irdaSensorPin4 );
bumperPinStateLeft = digitalRead(bumperPin1);
bumperPinStateRight = digitalRead(bumperPin2);
int randNumber = random(400, 700);

if (forwardON){vforward();} //
if (turbON) {motor1.run(BACKWARD); motor1.setSpeed(mSpeed3);} //
if (venikON) {motor2.run(BACKWARD); motor2.setSpeed(mSpeed4);} //

if (v == LOW && b == LOW && bumperPinStateRight == HIGH || v == LOW && b == LOW 
|| b == LOW && bumperPinStateRight == HIGH || v == LOW && bumperPinStateRight == HIGH)
{forwardON = false; vbackward(); bumper1ON = true; bumperRightWait = millis();}
if (n == LOW && m == LOW && bumperPinStateLeft == HIGH || n == LOW && b == LOW 
|| m == LOW && bumperPinStateLeft == HIGH || n == LOW && bumperPinStateLeft == HIGH)
{forwardON = false; vbackward(); bumper2ON = true; bumperLeftWait = millis();}

if (bumperPinStateRight == HIGH && bumperPinStateLeft == HIGH || v == LOW && m == LOW 
|| b == LOW && n == LOW || v == LOW && n == LOW || b == LOW && m == LOW)
{forwardON = false; vbackward(); allSensorON = true; allSensor = millis();}

if(allSensorON) {
if(millis() - allSensor > 600) {vrelease(); allSensorON = false;
  vright(); allSensor1ON = true; allSensor1 = millis();
  }
}
if(allSensor1ON) {if(millis() - allSensor1 > 1400) {vrelease(); allSensor1ON = false; forwardON = true;}}

//Бамперы ===============================================================================================
if (bumperPinStateRight == HIGH) {forwardON = false; vbackward(); bumper1ON = true; bumperRightWait = millis();} // Левый
if (bumperPinStateLeft == HIGH) {forwardON = false; vbackward(); bumper2ON = true; bumperLeftWait = millis();}   //Правый

//ЛЕВЫЙ===========================================================
if(bumper1ON) {
if(millis() - bumperRightWait > 300) {vrelease(); bumper1ON = false;
  vright(); waitTime1ON = true; waitTime1 = millis();
  }
}
if(waitTime1ON) {if(millis() - waitTime1 > randNumber) {vrelease(); waitTime1ON = false; forwardON = true;}}

//ПРАВЫЙ=========================================================
if(bumper2ON) {
if(millis() - bumperLeftWait > 300) {vrelease(); bumper2ON = false;
  vleft(); waitTime2ON = true; waitTime2 = millis();
  }
}
if(waitTime2ON) {if(millis() - waitTime2 > randNumber) {vrelease(); waitTime2ON = false; forwardON = true;}}

 //ИК датчики=================================================================================================
if (v == LOW) {forwardON = false; vright(); IRDAON1 = true; IRDA1 = millis();}   //1
if (b == LOW) {forwardON = false; vright(); IRDAON2 = true; IRDA2 = millis();}   //2
if (n == LOW) {forwardON = false; vleft(); IRDAON3 = true; IRDA3 = millis();}   //3
if (m == LOW) {forwardON = false; vleft(); IRDAON4 = true; IRDA4 = millis();}   //4

if(IRDAON1) {if(millis() - IRDA1 > 250) {vrelease(); IRDAON1 = false; forwardON = true;}}
if(IRDAON2) {if(millis() - IRDA2 > 500) {vrelease(); IRDAON2 = false; forwardON = true;}}
if(IRDAON3) {if(millis() - IRDA3 > 500) {vrelease(); IRDAON3 = false; forwardON = true;}}
if(IRDAON4) {if(millis() - IRDA4 > 250) {vrelease(); IRDAON4 = false; forwardON = true;}}

vcmd = (char)BTSerial.read(); 
Serial.println(vcmd);

if (BTSerial.available()) 
 {
  forwardON = false;
  if (vcmd == 'F') {vforward();} 
  if (vcmd == 'B') {vbackward();} 
  if (vcmd == 'L') {vleft();} 
  if (vcmd == 'R') {vright();} 
  //if (vcmd == 'G') {vforwardleft();} 
  //if (vcmd == 'I') {vforwardright();} 
  //if (vcmd == 'H') {vbackwardleft();} 
  //if (vcmd == 'J') {vbackwardright();} 
  if (vcmd == 'S') {vrelease();}
  if (vcmd == 'W') {forwardON = true;} 
 }
} // Конец основного цикла

// Функции ===============================================================

// Влево 
void vleft() {motor3.run(FORWARD); motor4.run(BACKWARD); 
motor3.setSpeed(mSpeed1); motor4.setSpeed(mSpeed1); }

// Вправо 
void vright() {motor3.run(BACKWARD); motor4.run(FORWARD); 
motor3.setSpeed(mSpeed1); motor4.setSpeed(mSpeed1); }

// Стоп 
void vrelease(){motor3.run(RELEASE); 
motor4.run(RELEASE); }

// Вперед  
void vforward() {motor3.run(FORWARD); motor4.run(FORWARD); 
motor3.setSpeed(mSpeed2); motor4.setSpeed(mSpeed2);}

// Назад 
void vbackward() {motor3.run(BACKWARD); motor4.run(BACKWARD); 
motor3.setSpeed(mSpeed1); motor4.setSpeed(mSpeed1);}

 

Верх