Arduino.ru

Здравствуйте.

Сделал машину с двумя датчиками HC-SR04.

Но к сожалению при езде они не срабатывают.

Посылаю по bluetooth команду вперед, если препятствия сразу перед машинкой то машина едет назад, а если машина уже эдит то датчик как будто не видит препятствия. та же ситуация при езде назад.

int ECHO_PIN1=4; //Forvard
int TRIGGER_PIN1=5; //Forvard
int ECHO_PIN2=2; //Reverse
int TRIGGER_PIN2=3; //Reverse
int cmForward=0;
int cmReverse=0;
int latchPin = 13;//pin is connectedST_CP входу 74HC595
int clockPin = 12;//pin is connected SH_CP входу 74HC595
int dataPin = 14;//pin is connected DS входу 74HC595
int latchPinLED = 16;//pin is connectedST_CP входу 74HC595
int clockPinLED = 17;//pin is connected SH_CP входу 74HC595
int dataPinLED = 15;//pin is connected DS входу 74HC595
int ena_a1=9;
int ena_b1=6;
int ena_a2=11;
int ena_b2=10;
int speed=200;
char direct; 
void setup() {
  Serial.begin(9600);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIGGER_PIN1, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  pinMode(TRIGGER_PIN2, OUTPUT);
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);
  pinMode(latchPinLED, OUTPUT);
  pinMode(clockPinLED, OUTPUT);
  pinMode(dataPinLED, OUTPUT);
  pinMode(ena_a1, OUTPUT);
  pinMode(ena_b1, OUTPUT);
  pinMode(ena_a2, OUTPUT);
  pinMode(ena_b2, OUTPUT);
  digitalWrite(TRIGGER_PIN1, LOW);
  digitalWrite(TRIGGER_PIN2, LOW);
  off_all();
}

///////////////////////measurement_distance//////////////
void measurement_distance(){
  digitalWrite(TRIGGER_PIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN1, LOW);
  cmForward=pulseIn(ECHO_PIN1, HIGH,20000)/58;
  //Serial.print("Measure_forward=");
 // Serial.println(cmForward);

  digitalWrite(TRIGGER_PIN2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN2, LOW);
  cmReverse=pulseIn(ECHO_PIN2, HIGH,20000)/58;
 // Serial.print("Measure_reverse=");
 // Serial.println(cmReverse);
}
//////////////////////////////////////////////////////////////////////
void forward(){
 // blue_all();
  Serial.println("FORWARD");
  digitalWrite(latchPin, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPin, clockPin, MSBFIRST, 170); // передаем последовательно на dataPin
  digitalWrite(latchPin, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах
  analogWrite(ena_a1, speed);
  analogWrite(ena_b1, speed);
  analogWrite(ena_a2, speed);
  analogWrite(ena_b2, speed);
}

void reverse(){
  Serial.println("REVERSE");
  digitalWrite(latchPin, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPin, clockPin, MSBFIRST, 85); // передаем последовательно на dataPin
  digitalWrite(latchPin, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах
//  white_all();  
  analogWrite(ena_a1, speed);
  analogWrite(ena_b1, speed);
  analogWrite(ena_a2, speed);
  analogWrite(ena_b2, speed); 
}

void right(){
  Serial.println("RIGHT");
  digitalWrite(latchPin, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPin, clockPin, MSBFIRST, 165); // передаем последовательно на dataPin
  digitalWrite(latchPin, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах
 // yellow_right();
  analogWrite(ena_a1, speed);
  analogWrite(ena_b1, speed);
  analogWrite(ena_a2, speed);
  analogWrite(ena_b2, speed); 
}
void left(){
  Serial.println("LEFT");
  digitalWrite(latchPin, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPin, clockPin, MSBFIRST, 90); // передаем последовательно на dataPin
  digitalWrite(latchPin, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах
//  yellow_left() ; 
  analogWrite(ena_a1, speed);
  analogWrite(ena_b1, speed);
  analogWrite(ena_a2, speed);
  analogWrite(ena_b2, speed); 
}

void stopp(){
  Serial.println("STOP");
  digitalWrite(latchPin, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPin, clockPin, MSBFIRST, 00); // передаем последовательно на dataPin
  digitalWrite(latchPin, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
  // red_all();
}
void red_left() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 254); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void blue_left() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 251); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void green_left() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 253); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void yellow_left() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 252); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void red_right() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 247); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void blue_right() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 239); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void green_right() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 223); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void yellow_right() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 215); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void red_all() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 246); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void blue_all() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 235); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void green_all() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 221); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void yellow_all() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 212); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void white_all() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 0); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}
void off_all() {
  digitalWrite(latchPinLED, LOW);// устанавливаем синхронизацию "защелки" на LOW
  shiftOut(dataPinLED, clockPinLED, MSBFIRST, 255); // передаем последовательно на dataPin
  digitalWrite(latchPinLED, HIGH); //"защелкиваем" регистр, тем самым устанавливая значения на выходах  
}

void loop() {
  if (Serial.available() > 0) {  //если есть доступные данные считываем байт
    direct = Serial.read();
    Serial.flush();
    switch (direct) {
    case '1':
      forward();
      break;
    case '2':
      reverse();
      break;
    case '3':
      left();
      break;
    case '4':
      right();
      break;   
    case '0':
      stopp();
      break;  
    case '5':
      stopp();
      break; 
    }
  }
  measurement_distance();
  if (cmForward<100 && direct=='1'){
    direct='2';
    reverse();
  }
  if (cmReverse<100 && direct=='2'){
    direct='1';
    forward();
  }
}