HC-SR04 + Uno + Motor Shield

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

Здравствуйте! Нужна помощь, сижу сейчас разбираюсь с платками, благодаря дельным советам, даденых мне, написал пару прстеньких прог, сейчас хочу сделать с Motor Shieldом и сонарами простенькую объезжалку препятствий, дайте совета, кода или ещё чего-нибудь =) если не трудно

maksim
Offline
Зарегистрирован: 12.02.2012

Первые же ссылки по запросу "робот" в поиске по сайту

images?q=tbn:ANd9GcSgFkqtCNuLV3tNRv9qqna
8 май 2012 ... Собрал робота на Ардуино Уно с ИК дальномером.
Так как являюсь новичком в програмировании и робототехнике, то и проект ...
arduino.ru/.../arduino-robot-dlya-nachinayushchikh
images?q=tbn:ANd9GcRpPqli5U7UsuEGpLpKF2i
14 май 2012 ... И вот почти тот самый робот, который объезжает препятствия и
пытается выехать из закрытого пространства.
arduino.ru/forum/proekty/robot-obezzhaika-1

 

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

ничего не выходит, никак не могу код под Motor Shield и HC-SR04 переделать, 4 раза уже изменял всё - дребедень, уже и стёр всё =(

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

Уважаемый maksim помогите нубу.

maksim
Offline
Зарегистрирован: 12.02.2012

Так что не получается??? Код покажите.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

вот в том-то и дело, что всё не идёт, я уже свои заготовки от злости удалил, а пытался переделать вот это:

#include <Servo.h> 
 #include <LMotorShield.h> 

 LMotorShield lms; 
 int gp2; 
 int gp3; 
 int gp4; 
 int gp5; 
 int gp24; 
 int gp25; 

 void setup() { 
 { 
 Serial.begin(9600); 
 lms.begin(LMS_SERVOS); 
 } 
 pinMode(3,OUTPUT); // Motor A скорость 
 pinMode(7,OUTPUT); // Motor A направление 
 pinMode(11,OUTPUT); // Motor B скорость 
 pinMode(8,OUTPUT); // Motor B направление 
 pinMode(2,OUTPUT); // Motor A2 
 pinMode(4,OUTPUT); // Motor B1 
 } 
 void Forward() { // Подпрограмма движения робота вперед 
 digitalWrite(3,HIGH); 
 digitalWrite(7,LOW); 
 digitalWrite(11,HIGH); 
 digitalWrite(8,LOW); 
 } 
 void Backward(){ // Подпрограмма движения робота назад 
 digitalWrite(3,HIGH); 
 digitalWrite(7,HIGH); 
 digitalWrite(11,HIGH); 
 digitalWrite(8,HIGH); 
 } 
 void motorStop(){ // Подпрограмма остановки мотора 

 digitalWrite(4,HIGH); 
 digitalWrite(2,HIGH); 
 } 
 void motorRun(){ // Подпрограмма запуска мотора 

 digitalWrite(2,LOW); 
 digitalWrite(4,LOW); 
 } 
 void Spin_Left(){ // Вращение в лево 
 digitalWrite(11,HIGH); 
 digitalWrite(8,LOW); 
 digitalWrite(3,HIGH); 
 digitalWrite(7,HIGH); 
 } 
 void Spin_Right(){ // Вращение в право 
 digitalWrite(3,HIGH); 
 digitalWrite(7,LOW); 
 digitalWrite(11,HIGH); 
 digitalWrite(8,HIGH); 
 } 
 void loop() 
 { 
 int i; 
 for (i=0;i<5;i++){ // Повторить 5 раз для фильтрации шума 
 gp2=(gp2+analogRead(0)); 
 } 
 gp2=gp2/5; 
 gp3=(2914/(gp2+5))+1; 
 Serial.println(gp3); 

 if (gp3<10){ 
 motorStop(); 

 lms.servoWrite(1,90); 
 delay(500); 

 lms.servoWrite(1,10); 
 delay(500); 
 for (i=0;i<5;i++){ // Повторить 5 раз для фильтрации шума 
 gp24=(gp24+analogRead(0)); 
 } 
 gp24=gp24/5; 
 gp4=(2914/(gp24+5))+2; 
 Serial.println(gp4); 

 lms.servoWrite(1,175); 
 delay(500); 
 int i; 
 for (i=0;i<5;i++){ // Повторить 5 раз для фильтрации шума 
 gp25=(gp25+analogRead(0)); 
 } 
 gp25=gp25/5; 
 gp5=(2914/(gp25+5))+2; 
 Serial.println(gp5); 

 lms.servoWrite(1,90); 
 delay(500); 

 if (gp4<12&&gp5<12){ 
 motorRun(); 
 Backward(); 
 delay(1000); 
 Spin_Left(); 
 delay(1118); 
 } 
 else { 
 if (gp4<12){ 
 motorRun(); 
 Forward(); 
 delay(300); 
 Spin_Left(); 
 delay(569); 
 } 
 if (gp5<12){ 
 motorRun(); 
 Forward(); 
 delay(300); 
 Spin_Right(); 
 delay(569); 
 } 
 if (gp4>12&&gp5>12){ 
 motorRun(); 
 Spin_Left(); 
 delay(569); 
 }}} 
 else 
 { 
 motorRun(); 
 Forward(); // Двигаться вперед 
 }}

 

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

пробовал заменить на motor shield и привязать sr04 - ничего не выходит =(

maksim
Offline
Зарегистрирован: 12.02.2012

Откуда вы вообще эту библиотеку взяли... 

http://arduino.ru/forum/apparatnye-voprosy/motor-shield-dk-electronics

1. Вставьте мотор-шилд в саму дуину! Уберите эти провода и сенсор-шилд вообще.
2. Качаете библиотеку AFMotor и распаковываете ее в Дуина\arduino-1.0.3\libraries\ ,только папка переименуйте в AFMotor, то есть у вас должна быть папка Дуина\arduino-1.0.3\libraries\AFMotor, в которой лежат файлы AFMotor.h, AFMotor.cpp и т.д.
3. Запускаете ArduinoIDE и пробуете пример: AFMotor -> MotorTest, выбрав соответствующий двигатель.

 

maksim
Offline
Зарегистрирован: 12.02.2012

Далее. В углу мотор-шилда есть 6 штырьков для подключения серв, к ним подключаете датчики:
VCC  -->  +
GND  -->  -
Trig  -->  S 
а Echo как-нибудь подключаете к А0 и А1.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

да не, это всё я делал, и сам сенсор в связке с шилдом работает замечательно, вопрос в том, что я не могу написать сам код, который будет отвечать за поворот влево/вправо/вперёд/назад, чтобы объезжать препятствия, поэтому взял вышеизложенный код и пробовал его изменить, удалив всё ненужное и добавив вместо ИК датчика -  sr04 и motor шилд, но ничего не получается

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

вот код из проекта "объезжайка-1"

#include <AFMotor.h>

#define DIST_DIR 40  //
#define DIST_ANG 150 // 
#define SENS_BAL 80 // 

#define Trig_pin A4
#define Echo_pin A5
#define Radar_pin A0
#define LED 13

AF_DCMotor R_motor(3);
AF_DCMotor L_motor(4);

int M, R, L, S, IR_R, IR_L, ang, dist, ss = 0;
byte distant[148];
long dist_cm, s_r, s_l, s, ang_sum = 0;
boolean led, dir = 0;
volatile int angle = 0;
unsigned long millis_old_Ir, millis_old_Ult, millis_old_LED, millis_old_dir, micros_old_Ult = 0;

void setup(){
  Serial.begin(9600);
  pinMode(LED, OUTPUT);
  pinMode(Trig_pin, OUTPUT);
  digitalWrite(2, HIGH);
  attachInterrupt(0, circle, RISING);
}

void loop(){ 
  IR_radar();
  Ultrasound();

  if(millis()-millis_old_dir > 200){

    M = map(dist_cm, 5, DIST_DIR, -100, 50);
    M = constrain(M, -100, 50);

    ang = map(dist_cm, 0, DIST_ANG, 65, 20);
    ang = constrain(ang, 20, 65);

    ang_sum = ang_sum*4/5+ang;
    ang = ang_sum/5;
    
    ss = map(S, 40, SENS_BAL, 30, 50);
    ss = constrain(ss, 30, 50);

    int i = 0;
    while(i < ang){
      s_r += distant[82+i];
      s_l += distant[76-i];
      i++;
    }
    i = 0;
     while(i < 148){
      s += distant[i];
      i++;
    }
    S = s/148;
    IR_R = s_r/ss;
    IR_L = s_l/ss;
    s_r = 0;
    s_l = 0;
    s = 0;
    if(IR_L > IR_R){
      R = M + IR_L/5;
      L = M - IR_R/5;
    }
    else{
      R = M - IR_L/5;
      L = M + IR_R/5;
    }

    Motors(R, L);
    millis_old_dir = millis();
  }
  
  Led();
}

void Ultrasound(){
  if(millis()-millis_old_Ult > 100){
    digitalWrite(Trig_pin, HIGH);
    delayMicroseconds(4);
    digitalWrite(Trig_pin, LOW);
    micros_old_Ult = micros();
    while(!digitalRead(Echo_pin) && micros()-micros_old_Ult < 500){
    }
    micros_old_Ult = micros();
    while(digitalRead(Echo_pin) && micros()-micros_old_Ult < 20000){
    }
    dist_cm = (micros() - micros_old_Ult)/29.0/2;
    millis_old_Ult = millis();
  }
}

void IR_radar(){
  if(millis()-millis_old_Ir > 5){
    int sens = 14145/(analogRead(Radar_pin)+7);
    sens = min(sens, 150);
    distant[angle] = sens;
    angle++;
    angle = min(angle, 148);
    millis_old_Ir = millis();
  }
}

void circle() {
  angle = 0; 
  led = 1;
  millis_old_LED = millis();
}

void Motors(int R, int L){
  int speed_R = 0;
  int speed_L = 0; 
  R = constrain(R, -100, 100);
  L = constrain(L, -100, 100);

  if(R > 9){
    R_motor.run(FORWARD);
    speed_R = map(R, 10, 100, 100, 255);
  }
  else if(R < -9){
    R_motor.run(BACKWARD);
    speed_R = map(R, -10, -100, 100, 255);
  }
  else{
    R_motor.run(RELEASE);
  }
  R_motor.setSpeed(speed_R);

  if(L > 9){
    L_motor.run(FORWARD);
    speed_L = map(L, 10, 100, 100, 255);
  }
  else if(L < -9){
    L_motor.run(BACKWARD);
    speed_L = map(L, -10, -100, 100, 255);
  }
  else{
    L_motor.run(RELEASE);
  }
  L_motor.setSpeed(speed_L);
}

void Led(){
  if(led){
    digitalWrite(LED, led);
    if(millis()-millis_old_LED > 200){
      led = 0;
      digitalWrite(LED, led);
      Serial.print(S, DEC);
      Serial.print(" ");
      Serial.print(IR_R/2, DEC);
      Serial.print(" ");
      Serial.println(IR_L/2, DEC);
      millis_old_LED = millis();
    }
  }
}

тут уже всё готово, но так как у меня нет ИК датчика, мне надо его как то удалить из кода и поместить sr04, но вот как? не могу переделать =(

maksim
Offline
Зарегистрирован: 12.02.2012

Если датчики подключите как написано в сообщении #8, а двигатели к М1 и М2, то этот код должен работать:

#include <AFMotor.h>

#define DIST 55  // расстояние действия сенсоров

#define LED 13

#define Trig_R 9
#define Echo_R A0
#define Trig_L 10
#define Echo_L A1

AF_DCMotor R_motor(1);
AF_DCMotor L_motor(2);

void setup()
{
  Serial.begin(9600);
  pinMode(LED, OUTPUT);
  pinMode(Trig_R, OUTPUT);
  pinMode(Trig_L, OUTPUT);
}

void loop()
{ 
  static unsigned long millis_prev;
  if(millis()-millis_prev > 100)
  {
    digitalWrite(LED, HIGH);
    
    int dist_R = Ultrasonic(Trig_R, Echo_R); // Измеряем расстояние справа
    int dist_L = Ultrasonic(Trig_L, Echo_L); // Измеряем расстояние слева 
    
    Motor(dist_R, L_motor);              // Управление левым мотором
    Motor(dist_L, R_motor);              // Управление правым мотором
    
    digitalWrite(LED, LOW);
    millis_prev = millis();
  }
}

int Ultrasonic(byte Trig_pin, byte Echo_pin)
{
  digitalWrite(Trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig_pin, LOW);   
  return pulseIn(Echo_pin, HIGH, 20000)/29.0/2;
}


void Motor(int dist, AF_DCMotor motor)
{
  dist = map(dist, 5, DIST, -100, 100);
  dist = constrain(dist, -100, 100);
  int speed = 0; 
  if(dist >= 10){
    motor.run(FORWARD);
    speed = map(dist, 10, 100, 100, 255);
  }
  else if(dist <= -10){
    motor.run(BACKWARD);
    speed = map(dist, -10, -100, 100, 255);
  }
  else{
    motor.run(RELEASE);
  }
  motor.setSpeed(speed);
}

Не перепутайте датчики слева и справа иначе робот наоборот будет ехать к препятсятвию.
Алгоритм очень простой - без сглаживаний и усреднений.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

огромное спасибо, сейчас испытаю

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

отлично! крутится, сейчас попробую поставить на саму платформу

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

предется наверно ещё что нить мне изменять, я ведь ставлю на платформу от РУ машины, а там же тип движения ни как у танка =(

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

блин, мотор "поёт", микруха греется, двигается только если поднести руку к левому датчику

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

так, осталось только настроить движение

triada13
Offline
Зарегистрирован: 04.01.2013

maksim подскажите нужно ли мне исплзовать библиотеку если да то какую, если я использую самодельный мотор шилд собраный вот по этой схеме?

 

maksim
Offline
Зарегистрирован: 12.02.2012

Нет, не нужно, но вы из этой библиотеки можете посмотреть как увеличить частоту ШИМа, что бы двигатели не пищали.

triada13
Offline
Зарегистрирован: 04.01.2013

они у меня пищат когда я выбираю от 50 до 150, а выше уже не пищат.

maksim
Offline
Зарегистрирован: 12.02.2012

А можно сделать что бы вообще не пищали.

triada13
Offline
Зарегистрирован: 04.01.2013

maksim а подскажите слепому, куда глядеть, ну чтоб не пищали.

maksim
Offline
Зарегистрирован: 12.02.2012

Так а дуина какая?

triada13
Offline
Зарегистрирован: 04.01.2013

UNO R3

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

мдаааа, круто =( avrdude: stk500_getsync(): not in sync: resp=0x00

maksim
Offline
Зарегистрирован: 12.02.2012

Что бы задействовать только один таймер (TIMER2) для управления двигателями ,подключите EN_A и EN_B к 11 и 3 выводам. 

void setup() 
{
  TCCR2A |= (1<<WGM20) | (1<<WGM21); // fast PWM
  TCCR2B = 1<<CS20;                                            // no prescale 64KHZ
  pinMode(11, OUTPUT); // M1
  pinMode(3, OUTPUT);  // M2
}

void loop() 
{
  M1_setSpeed(100);
  M2_setSpeed(15);
}

void M1_setSpeed(byte speed)
{
  if(speed) 
  {
    TCCR2A |= 1<<COM2A1;
    OCR2A = speed;
  }
  else TCCR2A &= ~(1<<COM2A1);
}

void M2_setSpeed(byte speed)
{
  if(speed) 
  {
    TCCR2A |= 1<<COM2B1;
    OCR2B = speed;  
  }
  else TCCR2A &= ~(1<<COM2B1);
}

Теперь частота ШИМа составляет 64 кГц, что в 3 раза превышает слышимый диапазон частот. Но учтите что не все драйвера могут работать на такой частоте.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

как же изменить код, что бы только один мотор крутил вперёд/назад, а не сразу два

maksim
Offline
Зарегистрирован: 12.02.2012

Не понимаю о чем вы, моторы крутятся независимо друг от друга.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

у меня платформа от РУ машинки, там тип движения НЕ как танковый, а код под танковый

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

тоесть для движения вперёд мне нужна работо ток 1 двигателя, а при таком коде работает сразу 2 и поэтому машина крутится

maksim
Offline
Зарегистрирован: 12.02.2012
#include <AFMotor.h>

#define DIST_MIN 10  // расстояние действия сенсоров
#define DIST_MAX 50  // расстояние действия сенсоров

#define LED 13

#define Trig_R 9
#define Echo_R A0
#define Trig_L 10
#define Echo_L A1

AF_DCMotor MOTOR(1);
AF_DCMotor RULE(2);

void setup()
{
  Serial.begin(9600);
  pinMode(LED, OUTPUT);
  pinMode(Trig_R, OUTPUT);
  pinMode(Trig_L, OUTPUT);
  RULE.setSpeed(255); 
}

void loop()
{ 
  static unsigned long millis_prev;
  if(millis()-millis_prev > 100)
  {
    digitalWrite(LED, HIGH);

    int dist_R = Ultrasonic(Trig_R, Echo_R); // Измеряем расстояние справа
    int dist_L = Ultrasonic(Trig_L, Echo_L); // Измеряем расстояние слева 

    if(dist_R > DIST_MIN && dist_L > DIST_MIN)
    {
      MOTOR.run(FORWARD);
      int speed = map((dist_R + dist_L)/2, DIST_MIN, DIST_MAX, 100, 255);
      MOTOR.setSpeed(constrain(speed, 100, 255));
      
      if(dist_R < DIST_MAX || dist_L < DIST_MAX)
      {
        if(dist_R < dist_L) RULE.run(FORWARD);
        if(dist_L < dist_R) RULE.run(BACKWARD);
        if(dist_L == dist_R)RULE.run(RELEASE);
      }
      else RULE.run(RELEASE);
    }
    else
    {
      RULE.run(RELEASE);
      MOTOR.run(BACKWARD);
      MOTOR.setSpeed(200);
      delay(2000); 
    }

    digitalWrite(LED, LOW);
    millis_prev = millis();
  }
}

int Ultrasonic(byte Trig_pin, byte Echo_pin)
{
  digitalWrite(Trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig_pin, LOW);   
  return pulseIn(Echo_pin, HIGH, 20000)/29.0/2;
}

 

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

спасибо

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

блин 293 не справляется, греется сильно, мож поверх неё ещё одну поставить?

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

да ну, за 20 сек она уже горячая =(

maksim
Offline
Зарегистрирован: 12.02.2012

Значит вы не соблюдаете токовый режим.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

l293d

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

неужели эти движки берут больше 600 ma

triada13
Offline
Зарегистрирован: 04.01.2013

Maksim спасибо, буду разбираться.

maksim
Offline
Зарегистрирован: 12.02.2012

triada13 пишет:

Maksim спасибо, буду разбираться.

Там ошибочка была

void setup() 
{
  TCCR2A |= (1<<WGM20) | (1<<WGM21); // fast PWM
  TCCR2B = 1<<CS20;                                            // no prescale 64KHZ
  pinMode(11, OUTPUT); // M1
  pinMode(3, OUTPUT);  // M2
}

void loop() 
{
  M1_setSpeed(100);
  M2_setSpeed(15);
}

void M1_setSpeed(byte speed)
{
  if(speed) 
  {
    TCCR2A |= 1<<COM2A1;
    OCR2A = speed;
  }
  else TCCR2A &= ~(1<<COM2A1);
}

void M2_setSpeed(byte speed)
{
  if(speed) 
  {
    TCCR2A |= 1<<COM2B1;
    OCR2B = speed;  
  }
  else TCCR2A &= ~(1<<COM2B1);
}

 

triada13
Offline
Зарегистрирован: 04.01.2013

maksim пишет:

Там ошибочка была

Ну вот так всегда ))))

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

вот это дела! перестал работать руль, код точно такой же как Вы давали, с другим кодом  работает....

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

блин, фигня какая-то твориться, не хочет ничего работать, поедет в стену и стоит

maksim
Offline
Зарегистрирован: 12.02.2012

Вы датчики куда направляете? Какой датчик левый какой правый?

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

правый к а0, левый к а1

maksim
Offline
Зарегистрирован: 12.02.2012

Ничего не видно.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

делаю всё, как Вы указали, все контакты как в коде

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

мож l293d почти сдох вот и чудит

maksim
Offline
Зарегистрирован: 12.02.2012

Сделайте фото поближе с разных сторон.

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

Вот

Tarantul_acc
Tarantul_acc аватар
Offline
Зарегистрирован: 09.03.2013

 

maksim
Offline
Зарегистрирован: 12.02.2012

Ни на одной из фото не видно как куда и где у вас подключен двигатель руля, и точно ли это двигатель, а не сервопривод?