Управление машинкой

ВериСлоу
Offline
Зарегистрирован: 02.10.2014

Вечер добрый.
Это программа для управления машинкой-роботом. 2 режима работы.
Что я хотел сделать...Хотел сделать так, чтобы первым режимом работы был режим управления по блютуз через телефон. и чтобы переключение на режим обхождения препятствий тоже осуществлялось нажатием кнопки на экране(в программе это строчка if (vcmd=='W') ). а возвращение к предыдущему режиму, ручному, нажатием другой кнопки (в программе строчка if (vcmd=='w') ). В итоге. при компиляции выдается ошибка:

robot.ino: In function 'void loop()':
robot.ino:60:23: error: a function-definition is not allowed here before '{' token
robot.ino:363:1: error: expected '}' at end of input
Ошибка компиляции

Погуглил... Пишут что то про функцию внутри функции. Исправить не смог. Поправьте пожалуйста кому не сложно код:

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

//Создаем объекты для двигателей 
AF_DCMotor motor1(1); //канал М1 на Motor Shield — задний левый 
AF_DCMotor motor2(2); //канал М2 на Motor Shield — задний правый 
// Создаем объект для сервопривода 
Servo vservo; 
// Прописываем пины используемые модулем Bluetooth 
SoftwareSerial BTSerial(2, 5); // RX, TX 
// Создаем переменную для команд Bluetooth 
char vcmd; 
// Создаем переменные для запоминания скорости левых и правых двигателей 
int vspdL, vspdR; 
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах. 
Текущая скорость должна быть больше этого значения.  В противном случае двигатели со стороны направления поворота просто не будут вращаться */ 
const int vspd = 200; 
// Создаем переменную для сохранения режима работы 
int vmode; 
// Создаем переменную для сохранения предыдущего режима работы 
int vmodeprev = -1; 
// Массив для хранения углов поворота сервопривода (шаг 15 градусов) 
const int vservo_array[13]={ 
  0,15,30,45,60,75,90,105,120,135,150,165,180}; 
// Массив для хранения данных о расстоянии под различными углами поворота сервопривода 
int vHC_SR04_array[13]; 
// Пины, используемые ультразвуковым дальномером 
const int vTrig = 6; 
const int vEcho = 10; 
// Переменные, для хранения данных с дальномера 
unsigned int vtime_us=0; 
unsigned int vdistance_sm=0; 
// Минимальное расстояние в сантиметрах, при котором нужно искать новый маршрут движения 
const int vmindistance = 30; 
// Переменная для циклов перебора значения массивов vservo_array и vHC_SR04_array 
int vservo_int; 
// Переменные для цикла поиска максимального значения в массивах 
int vmaxarrayindex_int; 
int vmaxarrayvalue_int;

void setup() { 
  // Устанавливаем скорость передачи данных по Bluetooth 
  BTSerial.begin(9600); 
  // Устанавливаем скорость передачи данных по кабелю 
  Serial.begin(9600); 
  // Выбираем пин к которому подключен сервопривод 
  vservo.attach(9); // или 10, если воткнули в крайний разъём 
  // Поворачиваем сервопривод в положение 90 градусов при каждом включении 
  vservo.write(90);  
  // Устанавливаем максимальную скорость вращения двигателей 
  vspeed(255,255); 
  // Устанавливаем значение для пинов, к которым подключен ультразвуковой дальномер 
  pinMode(vTrig, OUTPUT); 
  pinMode(vEcho, INPUT); 
}

void loop() { 
/* Режим управления через Bluetooth */ 
void vbluetoothmode() { 
  // Если есть данные с Bluetooth 
  if (BTSerial.available()) 
  { 
    /* Читаем команды и заносим их в переменную. 
     (char) преобразует код символа команды в символ */ 
    vcmd = (char)BTSerial.read(); 
    // Отправляем команду в порт, чтобы можно было их проверить в "Мониторе порта" 
    Serial.println(vcmd);
//Режим ручного управления
if (vcmd=='w'){
                                  //Начало  
 // Вперед 
    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); 
    } 
}
                                  //Конец
// Режим работы с использованием ультразвукового дальномера 
    if (vcmd == 'W') 
    { 
      /*Начало*/ 
void vultrasoundmode(){ 
  vservo.write(90);                  
  delay(200); 
  Serial.print("Now "); 
  Serial.println(vHC_SR04()); 
  // Если расстояние меньше наименьшего, то 
  if (vHC_SR04() < vmindistance) { 
    // Останавливаем двигатели 
    vrelease(); 
    // Крутим серву измеряя расстояния и занося данные в массив 
    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) { 
      vservo.write(vservo_array[vservo_int]); 
      delay(200); 
      vHC_SR04_array[vservo_int] = vHC_SR04(); 
      // Выводим данные для отладки 
      Serial.print(vservo_int); 
      Serial.print(" "); 
      Serial.println(vHC_SR04_array[vservo_int]); 
    } 
    vservo.write(90); 
    delay(500); 
    // Поиск в массиве позиции с максимальным значением 
    vmaxarrayindex_int = 0; 
    vmaxarrayvalue_int = 0;    
    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) {     
      if (vHC_SR04_array[vservo_int] > vmaxarrayvalue_int) { 
        vmaxarrayindex_int = vservo_int; 
        vmaxarrayvalue_int = vHC_SR04_array[vservo_int]; 
      } 
    } 
    Serial.print("Max index "); 
    Serial.println(vmaxarrayindex_int); 
    // Проверка - если максимальное значение массива меньше минимального расстояния, то едем назад 
    if (vHC_SR04_array[vmaxarrayindex_int] < vmindistance) { 
      vbackward(); 
      delay(500); 
    } 
    /* Проверка - если индекс максимального значения массива меньше 6 то поворачиваем вправо, 
    иначе влево */ 
    if (vmaxarrayindex_int < 6) { 
      vright(); 
      delay(500); 
    } 
    else 
    { 
      vleft(); 
      delay(500); 
    } 
  } 
  else 
  { 
    // Едем прямо 
    vforward(); 
  } 
}
/*Конец*/
    } 

  } 
}

/* Функция определения расстояния с дальномера */ 
int vHC_SR04() { 
  digitalWrite(vTrig, HIGH); // Подаем сигнал на выход микроконтроллера 
  delayMicroseconds(10); // Удерживаем 10 микросекунд 
  digitalWrite(vTrig, LOW); // Затем убираем 
  vtime_us=pulseIn(vEcho, HIGH); // Замеряем длину импульса 
  vdistance_sm=vtime_us/58; // Пересчитываем в сантиметры 
  return vdistance_sm; // Возвращаем значение 
}

/* Функции управления двигателями */

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

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

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

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

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

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

// Вперед и влево 
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(){ 
  motor1.run(RELEASE); 
  motor2.run(RELEASE); 
}

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

 

 

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

смотри строки 58 и 60. внутри одной функции объявил еще одну. это неправильно. объявлять отдельно, а вот вызвать пожалуйста

компилятор тебе написал ошибку

ВериСлоу
Offline
Зарегистрирован: 02.10.2014

о, поправил. вроде компилируется. но все равно. люди знающие перепроверьте пжлста.. :)
 

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

//Создаем объекты для двигателей 
AF_DCMotor motor1(1); //канал М1 на Motor Shield — задний левый 
AF_DCMotor motor2(2); //канал М2 на Motor Shield — задний правый 
// Создаем объект для сервопривода 
Servo vservo; 
// Прописываем пины используемые модулем Bluetooth 
SoftwareSerial BTSerial(2, 5); // RX, TX 
// Создаем переменную для команд Bluetooth 
char vcmd; 
// Создаем переменные для запоминания скорости левых и правых двигателей 
int vspdL, vspdR; 
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах. 
Текущая скорость должна быть больше этого значения.  В противном случае двигатели со стороны направления поворота просто не будут вращаться */ 
const int vspd = 200; 
// Создаем переменную для сохранения режима работы 
int vmode; 
// Создаем переменную для сохранения предыдущего режима работы 
int vmodeprev = -1; 
// Массив для хранения углов поворота сервопривода (шаг 15 градусов) 
const int vservo_array[13]={ 
  0,15,30,45,60,75,90,105,120,135,150,165,180}; 
// Массив для хранения данных о расстоянии под различными углами поворота сервопривода 
int vHC_SR04_array[13]; 
// Пины, используемые ультразвуковым дальномером 
const int vTrig = 6; 
const int vEcho = 10; 
// Переменные, для хранения данных с дальномера 
unsigned int vtime_us=0; 
unsigned int vdistance_sm=0; 
// Минимальное расстояние в сантиметрах, при котором нужно искать новый маршрут движения 
const int vmindistance = 30; 
// Переменная для циклов перебора значения массивов vservo_array и vHC_SR04_array 
int vservo_int; 
// Переменные для цикла поиска максимального значения в массивах 
int vmaxarrayindex_int; 
int vmaxarrayvalue_int;

void loop(){}

void setup() { 
  // Устанавливаем скорость передачи данных по Bluetooth 
  BTSerial.begin(9600); 
  // Устанавливаем скорость передачи данных по кабелю 
  Serial.begin(9600); 
  // Выбираем пин к которому подключен сервопривод 
  vservo.attach(9); // или 10, если воткнули в крайний разъём 
  // Поворачиваем сервопривод в положение 90 градусов при каждом включении 
  vservo.write(90);  
  // Устанавливаем максимальную скорость вращения двигателей 
  vspeed(255,255); 
  // Устанавливаем значение для пинов, к которым подключен ультразвуковой дальномер 
  pinMode(vTrig, OUTPUT); 
  pinMode(vEcho, INPUT); 
}


/*Начало*/ 
void vultrasoundmode(){ 
  vservo.write(90);                  
  delay(200); 
  Serial.print("Now "); 
  Serial.println(vHC_SR04()); 
  // Если расстояние меньше наименьшего, то 
  if (vHC_SR04() < vmindistance) { 
    // Останавливаем двигатели 
    vrelease(); 
    // Крутим серву измеряя расстояния и занося данные в массив 
    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) { 
      vservo.write(vservo_array[vservo_int]); 
      delay(200); 
      vHC_SR04_array[vservo_int] = vHC_SR04(); 
      // Выводим данные для отладки 
      Serial.print(vservo_int); 
      Serial.print(" "); 
      Serial.println(vHC_SR04_array[vservo_int]); 
    } 
    vservo.write(90); 
    delay(500); 
    // Поиск в массиве позиции с максимальным значением 
    vmaxarrayindex_int = 0; 
    vmaxarrayvalue_int = 0;    
    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) {     
      if (vHC_SR04_array[vservo_int] > vmaxarrayvalue_int) { 
        vmaxarrayindex_int = vservo_int; 
        vmaxarrayvalue_int = vHC_SR04_array[vservo_int]; 
      } 
    } 
    Serial.print("Max index "); 
    Serial.println(vmaxarrayindex_int); 
    // Проверка - если максимальное значение массива меньше минимального расстояния, то едем назад 
    if (vHC_SR04_array[vmaxarrayindex_int] < vmindistance) { 
      vbackward(); 
      delay(500); 
    } 
    /* Проверка - если индекс максимального значения массива меньше 6 то поворачиваем вправо, 
    иначе влево */ 
    if (vmaxarrayindex_int < 6) { 
      vright(); 
      delay(500); 
    } 
    else 
    { 
      vleft(); 
      delay(500); 
    } 
  } 
  else 
  { 
    // Едем прямо 
    vforward(); 
  } 
}
/*Конец*/


void vbluetoothmode() { 
  // Если есть данные с Bluetooth 
  if (BTSerial.available()) 
  { 
    /* Читаем команды и заносим их в переменную. 
     (char) преобразует код символа команды в символ */ 
    vcmd = (char)BTSerial.read(); 
    // Отправляем команду в порт, чтобы можно было их проверить в "Мониторе порта" 
    Serial.println(vcmd);
//Режим ручного управления
if (vcmd=='w'){
                                  //Начало  
 // Вперед 
    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); 
    } 
}
                                  //Конец
                                                                       
                                  
// Режим работы с использованием ультразвукового дальномера 
    if (vcmd == 'W') 
    { 
     vultrasoundmode(); 
    } 

  } 
}


/* Функция определения расстояния с дальномера */ 
int vHC_SR04() { 
  digitalWrite(vTrig, HIGH); // Подаем сигнал на выход микроконтроллера 
  delayMicroseconds(10); // Удерживаем 10 микросекунд 
  digitalWrite(vTrig, LOW); // Затем убираем 
  vtime_us=pulseIn(vEcho, HIGH); // Замеряем длину импульса 
  vdistance_sm=vtime_us/58; // Пересчитываем в сантиметры 
  return vdistance_sm; // Возвращаем значение 
}

/* Функции управления двигателями */

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

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

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

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

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

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

// Вперед и влево 
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(){ 
  motor1.run(RELEASE); 
  motor2.run(RELEASE);  
}

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

 

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

так в лупе ничего нет. ничего и не будет выполнятся))

ВериСлоу
Offline
Зарегистрирован: 02.10.2014

Елочки зеленые, точно... нужно как то туда впихнуть условие что если на андроиде нажата одна кнопка, то выполнять это, иначе то...
Только как...(
не могу что то понять

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

это загадка. отгадаешь заработает. решается для начала просто перебором, и от простого к сложному. зато разберешься

ВериСлоу
Offline
Зарегистрирован: 02.10.2014

Не могу я уже... сижу который час пытаюсь что нб сообразить... Не могу добиться того, чтобы по команде с телефона, первый режим выключался, и включался второй режим и наоборот, либо совсем не реагирует, либо не выключается вовсе... может все таки кто поможет?
Нажал кнопку, на ардуино пошел сигнал, включился второй режим, нажал еще раз, второй режим выключился и больше не ввключается пока я ему не скажу...
Ну если не прогой, то хоть идеей хорошей помогите...

 

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

const int SONIC_DISTANCE_MAX = 450;
const int SONIC_DISTANCE_MIN = 2;
const int DST_STOP = 20;
boolean SAFE_DISTANCE;// измерение дистанции и безопасное движение. Если впереди близко препятствие, то стоп
boolean robo; //режим обхождения препятствий
int distance = 0;

//Создаем объекты для двигателей 
AF_DCMotor motor1(1); //канал М1 на Motor Shield — задний левый 
AF_DCMotor motor2(2); //канал М2 на Motor Shield — задний правый 
AF_DCMotor motor3(3); //канал М3 на Motor Shield — передний левый 
AF_DCMotor motor4(4); //канал М4 на Motor Shield — передний правый 
// Создаем объект для сервопривода 
Servo vservo;  
// Создаем переменную для команд Bluetooth 
char vcmd; 
// Создаем переменные для запоминания скорости левых и правых двигателей 
int vspdL, vspdR; 
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах. 
Текущая скорость должна быть больше этого значения.  В противном случае двигатели со стороны направления поворота просто не будут вращаться */ 
const int vspd = 200; 
// Создаем переменную для сохранения режима работы 
int vmode; 
// Создаем переменную для сохранения предыдущего режима работы 
int vmodeprev = -1; 
// Массив для хранения углов поворота сервопривода (шаг 15 градусов) 
const int vservo_array[13]={ 
  0,15,30,45,60,75,90,105,120,135,150,165,180}; 
// Массив для хранения данных о расстоянии под различными углами поворота сервопривода 
int measureDistance_array[13]; 
// Пины, используемые ультразвуковым дальномером 
// 14, 15 аналоговые пины A0 и A1 соответственно
const int vTrig = 14; 
const int vEcho = 15; 
// Переменные, для хранения данных с дальномера 
unsigned int vtime_us=0; 
unsigned int vdistance_sm=0; 
// Минимальное расстояние в сантиметрах, при котором нужно искать новый маршрут движения 
const int vmindistance = 30; 
// Переменная для циклов перебора значения массивов vservo_array и measureDistance_array 
int vservo_int; 
// Переменные для цикла поиска максимального значения в массивах 
int vmaxarrayindex_int; 
int vmaxarrayvalue_int;

void setup() { 
   // Устанавливаем скорость передачи данных по кабелю 
  Serial.begin(9600); 
  // Выбираем пин к которому подключен сервопривод 
  vservo.attach(10); // или 10, если воткнули в крайний разъём 
  // Поворачиваем сервопривод в положение 90 градусов при каждом включении 
  vservo.write(90);  
  // Устанавливаем максимальную скорость вращения двигателей 
  vspeed(255,255); 
  // Устанавливаем значение для пинов, к которым подключен ультразвуковой дальномер 
  pinMode(vTrig, OUTPUT); 
  pinMode(vEcho, INPUT); 
}

void loop(){
// Если есть данные с Bluetooth 
  if (Serial.available()) 
  {  /* Читаем команды и заносим их в переменную. 
     (char) преобразует код символа команды в символ */ 
    vcmd = (char)Serial.read(); 
    // Отправляем команду в порт, чтобы можно было их проверить в "Мониторе порта" 
    Serial.println(vcmd);
//Начало  
 // Вперед 
    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(); 
    }  
     
    // Скорость 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); 
    } 

    if (vcmd=='V')
    {// ВКЛючить измерение дистанции и безопасное движение (в оригинале - гудок)
      SAFE_DISTANCE = true;
    }
              
    if (vcmd=='v')
    { // ВЫКЛючить измерение дистанции и безопасное движение (в оригинале - гудок)
      SAFE_DISTANCE = false;
    }
  
  //Далее мои неудачные попытки что то реализовать   
  /*  if (vcmd=='X')
    { //Включить режим обхождения препятствий. Аварийка
      robo = true;
    } 
               
    if (vcmd=='x')
    { //Выключить режим обхождения препятствий. Аварийка
      robo = false;
    }            
    */    
        //Стоп 
    if (vcmd == 's'){
      vrelease();
    } 
                      //Конец

}  
}

// Возвращает расстояние до препятствия в сантиметрах
int measureDistance()  {
  long duration;
  int  distance;
  /* Для запуска передатчика нужно подать на Trig сигнал, длительностью 10мкс.
   * Передатчик который посылает 8 коротких импульсов с частотой 40kHz.
   * Приемник получает отраженный сигнал и на входе Echo генерируется сигнал,
   * длительность которого равна времени прохождения звукового сигнала.
   */
  digitalWrite(vTrig, LOW); // инициализация перед замером
  delayMicroseconds(5); // 3
  digitalWrite(vTrig, HIGH);
  delayMicroseconds(12); // 10
  digitalWrite(vTrig, LOW);

  duration = pulseIn(vEcho, HIGH);
  // Скорость звука 340 м/с или 29 микросекунд на сантиметр.
  // Звук идет вперед и возвращается назад, таким образом время нужно делить на два
  distance = duration / 58; // = microseconds / 29 / 2

  if (distance < SONIC_DISTANCE_MIN )  // out of range
    return SONIC_DISTANCE_MIN;
  if (distance > SONIC_DISTANCE_MAX )  // out of range
    return SONIC_DISTANCE_MAX;

  return distance;
}


/*Начало*/ 
void vultrasoundmode(){
  vservo.write(90);                  
  delay(200); 
  Serial.print("Now "); 
  Serial.println(measureDistance()); 
  // Если расстояние меньше наименьшего, то 
  if (measureDistance() < vmindistance) { 
    // Останавливаем двигатели 
    vrelease(); 
    // Крутим серву измеряя расстояния и занося данные в массив 
    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) { 
      vservo.write(vservo_array[vservo_int]); 
      delay(200); 
      measureDistance_array[vservo_int] = measureDistance(); 
      // Выводим данные для отладки 
      Serial.print(vservo_int); 
      Serial.print(" "); 
      Serial.println(measureDistance_array[vservo_int]); 
    } 
    vservo.write(90); 
    delay(500); 
    // Поиск в массиве позиции с максимальным значением 
    vmaxarrayindex_int = 0; 
    vmaxarrayvalue_int = 0;    
    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) {     
      if (measureDistance_array[vservo_int] > vmaxarrayvalue_int) { 
        vmaxarrayindex_int = vservo_int; 
        vmaxarrayvalue_int = measureDistance_array[vservo_int]; 
      } 
    } 
    Serial.print("Max index "); 
    Serial.println(vmaxarrayindex_int); 
    // Проверка - если максимальное значение массива меньше минимального расстояния, то едем назад 
    if (measureDistance_array[vmaxarrayindex_int] < vmindistance) { 
      vbackward(); 
      delay(500); 
    } 
    /* Проверка - если индекс максимального значения массива меньше 6 то поворачиваем вправо, 
    иначе влево */ 
    if (vmaxarrayindex_int < 6) { 
      vright(); 
      delay(500); 
    } 
    else 
    { 
      vleft(); 
      delay(500); 
    } 
  } 
  else 
  { 
    // Едем прямо 
    vforward(); 
  } 

                                                                                                                                                                                                                     
}
/*Конец*/

// безопасно ли ехать вперед?
boolean isSafeDistance() {
    if ( SAFE_DISTANCE )  {
        // замер расстояния
        distance = measureDistance();
        // препятствие так близко что надо ехать назад ?
        if ( distance < DST_STOP ) {
            return false;
        }
    } else {
        return true;
    }
}


/* Функции управления двигателями */

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

// Вперед для RL 
boolean vforwardRL() {
 if ( !isSafeDistance() )  {
      vrelease();
      return false;
  } 
  motor1.run(FORWARD); 
  motor2.run(FORWARD); 
  motor3.run(FORWARD); 
  motor4.run(FORWARD);
}

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

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

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

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

// Вперед и влево 
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(){ 
  motor1.run(RELEASE); 
  motor2.run(RELEASE); 
  motor3.run(RELEASE); 
  motor4.run(RELEASE); 
}

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

 

bwn
Offline
Зарегистрирован: 25.08.2014

А может выкинуть строк так 350 и просто светики с телефона позажигать?