Arduino.ru

#include <IRremote.h>
#include <AFMotor.h> // Подключаем библиотеку для управления двигателями 
#include <Servo.h>   // Подключаем библиотеку для сервоприводов  
#include <SoftwareSerial.h> // Подключаем библиотеку для работы с Serial через дискретные порты
int RECV_PIN = 51; //вход ИК приемника
IRrecv irrecv(RECV_PIN);
decode_results results;
int a=0; // переменная переключения режима, при нажатии кнопки она принимает значение +1
int b=0;//
int d=0;
byte aa = 0;
byte bb = 0;
byte dd = 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 
SoftwareSerial BTSerial(52, 53); // RX, TX 
// Создаем переменную для команд Bluetooth 
char vcmd;
const int light[2]={
  32,33};
const int turn_led[2]={
  34,35};
// Создаем переменные для запоминания скорости левых и правых двигателей 
int vspdL, vspdR; 
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах. 
Текущая скорость должна быть больше этого значения.  В противном случае двигатели со стороны направления поворота просто не будут вращаться */ 
const int vspd = 200; 
// Заносим в массив пины, к которым подключены светодиоды 
const int vledpins[3]={ 
  22,23,24}; 
// Создаем переменную для сохранения режима работы 
int vmode; 
// Создаем переменную для сохранения предыдущего режима работы 
int vmodeprev = -1; 
// Заносим в массив пины, к которым подключены кнопки 
const int vbtn[2]={ 
  28,29};
const int reset[1]={
  50};
// Массив для хранения углов поворота сервопривода (шаг 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 = 31; 
const int vEcho = 30; 
// Переменные, для хранения данных с дальномера 
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() { 
  irrecv.enableIRIn(); // включить приемник
pinMode(reset[0], OUTPUT);
pinMode(28, OUTPUT);
pinMode(29, OUTPUT);
pinMode(light[0], OUTPUT);
pinMode(light[1], OUTPUT);
pinMode(turn_led[0], OUTPUT);
pinMode(turn_led[1], OUTPUT);
  // Устанавливаем скорость передачи данных по Bluetooth 
  BTSerial.begin(9600); 
  // Устанавливаем скорость передачи данных по кабелю 
  Serial.begin(9600); 
  // Выбираем пин к которому подключен сервопривод 
  vservo.attach(9); // или 10, если воткнули в крайний разъём 
  // Поворачиваем сервопривод в положение 90 градусов при каждом включении 
  vservo.write(90);  
  // Устанавливаем максимальную скорость вращения двигателей 
  vspeed(255,255); 
  /* Устанавливаем все выводы, к которым подключены светодиоды, 
   в OUTPUT. Зажигаем и гасим светодиоды с интервалом в 0.5 сек 
   для проверки */ 
  for (vmode = 0; vmode < 3; vmode = vmode + 1) { 
    pinMode(vledpins[vmode], OUTPUT);  
    digitalWrite(vledpins[vmode], HIGH); 
    delay (500); 
    digitalWrite(vledpins[vmode], LOW); 
  } 
  /* Устанавливаем выводы, к которым подключены кнопки, в INPUT. */ 
  pinMode(vbtn[0], INPUT);  
  pinMode(vbtn[1], INPUT); 
  // Устанавливаем значение первого режима работы робота 
  vmode = 0; 
  // Устанавливаем значение для пинов, к которым подключен ультразвуковой дальномер 
  pinMode(vTrig, OUTPUT); 
  pinMode(vEcho, INPUT); 
}

void loop() {
 if (irrecv.decode(&results)) {
delay(300); // задержка перед выполнением определения кнопок, чтобы избежать быстрое двойное нажатие

if (results.value == 0xAF5F20D) {a=a+1; aa = 0;} else {aa++;}
if (results.value == 0xAF550AF) {b=b+1; bb = 0;} else {bb++;}
if (results.value == 0xAF56897) {d=d+1; dd = 0;} else {dd++;}
if (a==1){digitalWrite(28, HIGH);} 
if (aa == 3){digitalWrite(28, LOW); a = 0;}
if (b==1){digitalWrite(29, HIGH);}
if (bb == 3){digitalWrite(29, LOW); b = 0;}
if (d==1){digitalWrite(reset[0], HIGH);} 
if (dd == 3){digitalWrite(reset[0], LOW); d = 0;}
{ //
delay(10); //пауза между повторами //
} //
irrecv.resume(); // 

}

  /* Переключение режимов работы робота */ 
  // Кнопка переключения на следующий режим - BTN1 
  if (digitalRead(vbtn[0]) == HIGH){   
    vmode = 1;    
    delay (500); 
  }
  
  // Кнопка переключения на предыдущий режим - BTN2 
  if (digitalRead(vbtn[1]) == HIGH){   
    vmode = 2;    
    delay (500); 
  }
  // Засвечиваем светодиод текущего режима работы 
  digitalWrite(vledpins[vmode], HIGH);  
  /* Выбор режима работы */ 
  switch (vmode) { 
  case 1: 
    // Режим работы с использованием ультразвукового дальномера 
    vultrasoundmode(); 
    break; 
  case 2: 
      // Режим управления через Bluetooth 
    vbluetoothmode(); 
    break; 
  } 
} 
/* Режим работы с использованием ультразвукового дальномера */ 
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(); 
  } 
}

/* Режим управления через Bluetooth */ 
void vbluetoothmode() { 
  // Если есть данные с Bluetooth 
  if (BTSerial.available()) 
  { 
    /* Читаем команды и заносим их в переменную. 
     (char) преобразует код символа команды в символ */ 
    vcmd = (char)BTSerial.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(); 
    }  
    // Стоп 
    if (vcmd == 'S') 
    { 
      vrelease(); 
    }
   if (vcmd == 'W') 
    {
     vlighton();
    } 
   if (vcmd == 'w') 
    {
     vlightoff();
    }
   if (vcmd == 'U') 
    {
     v2lighton();
    } 
   if (vcmd == 'u') 
    {
     v2lightoff();
    } 
   if (vcmd == 'V') 
    {
     vturnlefton();
    }
   if (vcmd == 'v') 
    {
     vturnleftoff();
    }
   if (vcmd == 'X') 
    {
     vturnrighton();
    }
   if (vcmd == 'x') 
    {
     vturnrightoff();
    }    
    // Скорость 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);
    }
    
  } 
}

/* Функция определения расстояния с дальномера */ 
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); 
  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 vlighton(){
  digitalWrite(light[0], HIGH);
}
void vlightoff(){
  digitalWrite(light[0], LOW);
}
void v2lighton(){
  digitalWrite(light[1], HIGH);
}
void v2lightoff(){
  digitalWrite(light[1], LOW);
}
void vturnlefton(){
  digitalWrite(turn_led[0], HIGH);   
  delay(1000);              
  digitalWrite(turn_led[0], LOW);    
  delay(1000);
}  
void vturnleftoff(){
  digitalWrite(turn_led[0], LOW);   
  delay(1000);              
  digitalWrite(turn_led[0], LOW);    
  delay(1000);
}
void vturnrighton(){
  digitalWrite(turn_led[1], HIGH);   
  delay(1000);              
  digitalWrite(turn_led[1], LOW);    
  delay(1000);
}  
void vturnrightoff(){
  digitalWrite(turn_led[1], LOW);   
  delay(1000);              
  digitalWrite(turn_led[1], LOW);    
  delay(1000);
}  

// Изменение скорости 
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); 
}