опять робот ик датчик + HC-SR04 и сервомотор

artemon25
Offline
Зарегистрирован: 25.12.2015

Робот на ардуино это уже не новость собирал робот с помощью HC-SR04 объезжает препятсвия а так же другого робота с ик датчиками оба работают хорошо . НО всегда хочется чего то боьше собрал робота по примеру R2D2 звёздные войны и проблема в том что HC-SR04 датчик стоит в голове робота тоесть высоко и препятсвия которые вниу не видит . Поэтому возникла идея объединить 2 робота в один и добавить вниз ик датчики . но попытки объединить коды реультата не дали(слабый в програмировании) поэтому дам 2 кода с разнх роботов есть идеи как объединить скетчи чтоб работал и с HC-SR04 датчиком и с двумя ик датчиками

#include <Servo.h>          //Библиотека сервомотора. Есть по стандарту
#include <NewPing.h>        //Библиотека Ультразвукового датчика (нужно установить)

//Пины контроллера L298N
const int LeftMotorForward = 7;
const int LeftMotorBackward = 6;
const int RightMotorForward = 5;
const int RightMotorBackward = 4;

//Пины ультразвукового датчика
#define trig_pin A0 //Аналоговый вход 1
#define echo_pin A1 //Аналоговый вход 2

#define maximum_distance 200
boolean goesForward = false;
int distance = 100;

NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
Servo servo_motor; //our servo name


void setup(){

  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);
  
  servo_motor.attach(8); //Пин подключения сервомотора

  servo_motor.write(115);
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop(){

  int distanceRight = 0;
  int distanceLeft = 0;
  delay(50);

  if (distance <= 40){
    moveStop();
    delay(400);
    moveBackward();
    delay(500);
    moveStop();
    delay(400);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);

    if (distance >= distanceLeft){
      turnRight();
      moveStop();
    }
    else{
      turnLeft();
      moveStop();
    }
  }
  else{
    moveForward(); 
  }
    distance = readPing();
}

int lookRight(){  
  servo_motor.write(50);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
}

int lookLeft(){
  servo_motor.write(170);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
  delay(100);
}

int readPing(){
  delay(70);
  int cm = sonar.ping_cm();
  if (cm==0){
    cm=250;
  }
  return cm;
}

void moveStop(){
  
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
}

void moveForward(){

  if(!goesForward){

    goesForward=true;
    
    digitalWrite(LeftMotorForward, HIGH);
    digitalWrite(RightMotorForward, HIGH);
  
    digitalWrite(LeftMotorBackward, LOW);
    digitalWrite(RightMotorBackward, LOW); 
  }
}

void moveBackward(){

  goesForward=false;

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward, LOW);
  
}

void turnRight(){

  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  
  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
 
  
  
}

void turnLeft(){

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);

  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
}

и воторой скетч с ик датчиками 


int d_S; // поправка на компенсацию вращения правого двигателя
int S_auto; // задается скорость движения вперед
int del; // переменная задержки перед движением вперед
const int IN1 = 2; // определяем пины управления двигателями правый
const int IN2 = 4; // правый
const int IN3 = 7; // левый
const int IN4 = 8; // левый
const int EN1 = 5; // шим левого двигателя
const int EN2 = 6; // шим правого двигателя
const int ik_center = 11; // пин центрального ик-датчика
const int ik_left = 10; // пин левого ик-датчика
const int ik_right = 12; // пин правого датчика
int v_left; // переменные сработки датчиков
int v_center;
int v_right;

void setup()
{
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(EN1, OUTPUT);
  pinMode(EN2, OUTPUT);
  pinMode(ik_left, INPUT);
  pinMode(ik_center, INPUT);
  pinMode(ik_right, INPUT);
  
  S_auto = 95; // скорость движения вперед
  d_S = 5;  // поправка для компенсации правого двигателя
  del = 300; // значение функции делей
 
}

void loop(){
   v_left = digitalRead( ik_left ); 
     if( v_left == LOW ) { // если сработал левый датчик
     Back_Right(); // отъезжаем назад вправо
     delay(del);
   }
   else { // если нет, едем вперед
   Forward();
   
   } 
  v_center = digitalRead( ik_center );
   if( v_center == LOW ) { // если сработал центральный датчик
     Back_Left(); // едем назад влево
     delay(del);
   }
   else { // если нет - вперед
   Forward();
   } 
 v_right = digitalRead( ik_right );
   if( v_right == LOW ) { // если сработал правый датчик
     Back_Left(); // едем назад влево
     delay(del);
   }
   else { // если нет - вперед
   Forward();
   }     
} 

  
void Forward(){ // функция движения вперед
     digitalWrite (IN1, LOW);
      digitalWrite (IN2, HIGH); 
      digitalWrite (IN3, LOW);
      digitalWrite (IN4, HIGH);
      analogWrite(EN1, S_auto); 
      analogWrite(EN2, S_auto + d_S);
}
void Back_Left(){ // функция движения назад влево
      digitalWrite (IN1, HIGH);
      digitalWrite (IN2, LOW); 
      digitalWrite (IN3, HIGH);
      digitalWrite (IN4, LOW);
      analogWrite(EN1, 160); 
      analogWrite(EN2, 90);
}
void Back_Right(){ // функция движения назад вправо
      digitalWrite (IN1, HIGH);
      digitalWrite (IN2, LOW); 
      digitalWrite (IN3, HIGH);
      digitalWrite (IN4, LOW);
      analogWrite(EN1, 90); 
      analogWrite(EN2, 160);
}
в первом не задействованы шим двигателей впринципе это не так важно можно и без шим .
Буду рад любой помощи и подсказкам.
 
Заранее всем спасибо
ua6em
ua6em аватар
Offline
Зарегистрирован: 17.08.2016

Удалось победить?

artemon25
Offline
Зарегистрирован: 25.12.2015

скетч кое как сам написал но провериить пока не могу ик датчики не пришли с Китая

mykaida
mykaida аватар
Offline
Зарегистрирован: 12.07.2018

Уважаемый ТС, вы ни этот скетч пишите, а алгоритм. Поскольку в скетче Вы нихрена не понимаете, так может в алгоритме чего-нить поймете...

artemon25
Offline
Зарегистрирован: 25.12.2015

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

#include <Servo.h>          //Библиотека сервомотора. Есть по стандарту
#include <NewPing.h>        //Библиотека Ультразвукового датчика (нужно установить)

//Пины контроллера L298N
const int LeftMotorForward = 7;
const int LeftMotorBackward = 6;
const int RightMotorForward = 5;
const int RightMotorBackward = 4;

//Пины ультразвукового датчика
#define trig_pin A0 //Аналоговый вход 1
#define echo_pin A1 //Аналоговый вход 2

#define maximum_distance 200
boolean goesForward = false;
int distance = 100;
const int RightPin = 10; 
const int LeftPin = 11; 
int RightState = 0;   
int LeftState = 0;   



NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
Servo servo_motor; //our servo name


void setup(){

  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);
   pinMode(RightPin, INPUT);
   pinMode(LeftPin, INPUT);
  
  servo_motor.attach(8); //Пин подключения сервомотора

  servo_motor.write(115);
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop(){

  int distanceRight = 0;
  int distanceLeft = 0;
  delay(50);
  RightState = digitalRead(RightPin); 
  LeftState = digitalRead(LeftPin); 

    if (RightState == HIGH) {
    moveStop();
    delay(400);
    moveBackward();
    delay(500);
    moveStop();
    delay(400);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);
 if (distance >= distanceLeft){
      turnRight();
      moveStop();
    }
    else{
      turnLeft();
      moveStop();
    }
  }
  else{
    moveForward(); 
    delay(300);
     distance = readPing();
  }
  
 if (LeftState == HIGH) {
    moveStop();
    delay(400);
    moveBackward();
    delay(500);
    moveStop();
    delay(400);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);
 if (distance >= distanceLeft){
      turnRight();
      moveStop();
    }
    else{
      turnLeft();
      moveStop();
    }
  }
  else{
    moveForward(); 
    delay(300);
     distance = readPing();
  }
  

  if (distance <= 40){
    moveStop();
    delay(400);
    moveBackward();
    delay(500);
    moveStop();
    delay(400);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);

    if (distance >= distanceLeft){
      turnRight();
      moveStop();
    }
    else{
      turnLeft();
      moveStop();
    }
  }
  else{
    moveForward(); 
  }
    distance = readPing();
}

int lookRight(){  
  servo_motor.write(50);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
}

int lookLeft(){
  servo_motor.write(170);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
  delay(100);
}

int readPing(){
  delay(70);
  int cm = sonar.ping_cm();
  if (cm==0){
    cm=250;
  }
  return cm;
}

void moveStop(){
  
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
}

void moveForward(){

  if(!goesForward){

    goesForward=true;
    
    digitalWrite(LeftMotorForward, HIGH);
    digitalWrite(RightMotorForward, HIGH);
  
    digitalWrite(LeftMotorBackward, LOW);
    digitalWrite(RightMotorBackward, LOW); 
  }
}

void moveBackward(){

  goesForward=false;

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward, LOW);
  
}

void turnRight(){

  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  
  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
 
  
  
}

void turnLeft(){

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);

  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
}

но не знаю правильно или нет так как нет ик датчиков может  кто то проверит и иподскажет что нет ак