#include "Wire.h" // библиотека для управления I2C #include "HMC5883L.h" // библеотека для компаса #include // библиотека для работы с Serial через дискретные порты #include // библиотека для управления DC двигателями #include // библиотека для управления сервоприводами HMC5883L compass; // режим работаы HMC5883L #define LED_STOP_PIN 25 // контакт STOP-светодиода #define LED_BT_PIN 29 // контакт BT-светодиода #define LED_AUTO_PIN 28 // контакт AUTO-светодиода #define BUTTON_PIN 24 // контакт кнопки #define TRIGGER_PIN 26 // контакт уз излучателя #define ECHO_PIN 27 // контакт уз приемника 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 servo; // сервопривод SoftwareSerial BTSerial(A14, A15); // RX, TX для BT модуля char bt_cmd; // переменную для команд Bluetooth //int spdL, spdR; // переменные для запоминания скорости двигателей int distance; int distance_min = 20; int mode = 0; // переменная режима работы int microsec = 0; // время отражения сигнала const int servo_array[7]={2,25,50,80,110,135,158}; // Массив для хранения углов поворота сервопривода (шаг 30 градусов, 5 шагов) int HC_SR04_array[7]; // Массив для хранения данных о расстоянии под различными углами поворота сервопривода int servo_int; // Переменная для циклов перебора значения массивов vservo_array и vHC_SR04_array int maxarrayindex_int; // Переменные для цикла поиска максимального значения в массивах int maxarrayvalue_int; int compass_value; // показание компаса 1 int compass_value_2; // целевое показание компаса void setup() { Serial.begin(9600); // устанавливаем скорость передачи данных по кабелю BTSerial.begin(9600); // устанавливаем скорость передачи данных по Bluetooth Wire.begin(); servo.attach(9); // пин сервопривода (на плате расширения - SERVO2) servo.write(80); // поворот сервопривода по ходу движения (с учетом крена)servo_h.write(88); // поворот сервопривода по ходу движения (с учетом крена) pinMode(LED_STOP_PIN, OUTPUT); // режим работы контакта для светодиода pinMode(LED_BT_PIN, OUTPUT); // режим работы контакта для светодиода pinMode(LED_AUTO_PIN, OUTPUT); // режим работы контакта для светодиода pinMode(BUTTON_PIN, INPUT); // режим работы контакта для кнопки pinMode(TRIGGER_PIN, OUTPUT); // режим работы контакта для уз излучателя pinMode(ECHO_PIN, INPUT); // режим работы контакта для уз приемника compass = HMC5883L(); compass.SetScale(1.3); // установка чувствительности компаса compass.SetMeasurementMode(Measurement_Continuous); //устанавливаем непрерывный режим работы компаса delay(200); set_speed(255,255); } void loop(){ if (digitalRead(BUTTON_PIN) == HIGH){ mode = mode + 1; if (mode > 3){ mode = 0; } dc_release(); delay(1500); } switch (mode){ case 0: servo.detach(); digitalWrite(LED_BT_PIN, HIGH); digitalWrite(LED_AUTO_PIN, LOW); mode_bt(); // дистанционное управление break; case 1: set_speed(255,255); servo.attach(9); digitalWrite(LED_BT_PIN, LOW); digitalWrite(LED_AUTO_PIN, HIGH); mode_auto(); // автоматическая езда break; case 2: set_speed(255,255); servo.detach(); digitalWrite(LED_BT_PIN, HIGH); digitalWrite(LED_AUTO_PIN, HIGH); compass_test(); // тест компаса break; case 3: break; // режим ожидания } } void mode_auto(){ // автоматическая езда передом с вращением датчика servo.write(80); delay(100); if (u_distance() < distance_min) { dc_release(); // Останавливаем двигатели digitalWrite(LED_STOP_PIN, HIGH); // гажигаем светодиод for (servo_int = 0; servo_int < 7; servo_int = servo_int + 1) { // Крутим серву, измеряя расстояния и занося данные в массив servo.write(servo_array[servo_int]); delay(25); HC_SR04_array[servo_int] = u_distance(); } servo.write(80); delay(100); // Поиск в массиве позиции с максимальным значением maxarrayindex_int = 0; maxarrayvalue_int = 0; for (servo_int = 0; servo_int < 7; servo_int = servo_int + 1) { if (HC_SR04_array[servo_int] > maxarrayvalue_int) { maxarrayindex_int = servo_int; maxarrayvalue_int = HC_SR04_array[servo_int]; } } // Проверка - если максимальное значение массива меньше минимального расстояния, то зажигаем светодиод и едем назад if (HC_SR04_array[maxarrayindex_int] < distance_min) { digitalWrite(LED_STOP_PIN, HIGH); dc_backward(); delay(200); } // Проверка - если индекс максимального значения массива меньше 2 то поворачиваем вправо, иначе влево // if (maxarrayindex_int < 3) {dc_right(); delay(200);} else {dc_left(); delay(200);} } else {digitalWrite(LED_STOP_PIN, LOW); dc_forward();} // гасим светодиод и едем прямо } void mode_bt(){ // управление по bluetooth u_distance(); if (BTSerial.available()) // Если есть данные с BT { if (distance < distance_min) {dc_release(); digitalWrite(LED_STOP_PIN, HIGH);} // зажигаем светодиод else {digitalWrite(LED_STOP_PIN, LOW);} // гасим светодиод bt_cmd = (char)BTSerial.read(); // Читаем команды и заносим их в переменную. char преобразует код символа команды в символ // движение if (bt_cmd == 'F' && distance >= distance_min) {dc_forward();} if (bt_cmd == 'B') {dc_backward();} if (bt_cmd == 'L') {dc_left();} if (bt_cmd == 'R') {dc_right();} if (bt_cmd == 'S') {dc_release();} // управление скоростью if (bt_cmd == '0') {set_speed(115,115);} if (bt_cmd == '1') {set_speed(130,130);} if (bt_cmd == '2') {set_speed(145,145);} if (bt_cmd == '3') {set_speed(160,160);} if (bt_cmd == '4') {set_speed(175,175);} if (bt_cmd == '5') {set_speed(180,180);} if (bt_cmd == '6') {set_speed(195,195);} if (bt_cmd == '7') {set_speed(210,210);} if (bt_cmd == '8') {set_speed(225,225);} if (bt_cmd == '9') {set_speed(240,240);} if (bt_cmd == 'q') {set_speed(255,255);} // прочие телодвижения if (bt_cmd == 'X' || bt_cmd == 'x') {mode = 1; return;} // переход в авторежим } } void compass_test(){ // тест компаса compass_value = compass_measure() / 1.0; compass_value_2 = (compass_value + 90) % 360; delay(2500); while (!(compass_value >= compass_value_2 - 1.7 && compass_value <= compass_value_2 + 1.7)){ dc_right(); delay(27); dc_release(); delay(45); compass_value = compass_measure() / 1.0; } dc_release(); } // движение void dc_forward() { // Функция движения вперед motor1.run(FORWARD); motor2.run(FORWARD); motor3.run(FORWARD); motor4.run(FORWARD); } void dc_backward() { // Функция движения назад motor1.run(BACKWARD); motor2.run(BACKWARD); motor3.run(BACKWARD); motor4.run(BACKWARD); } void dc_right() { // Функция движения влево motor1.run(FORWARD); motor2.run(FORWARD); motor3.run(BACKWARD); motor4.run(BACKWARD); } void dc_left() { // Функция движения вправо motor1.run(BACKWARD); motor2.run(BACKWARD); motor3.run(FORWARD); motor4.run(FORWARD); } void dc_release(){ // Функция остановки motor1.run(RELEASE); motor2.run(RELEASE); motor3.run(RELEASE); motor4.run(RELEASE); } void set_speed(int spdL,int spdR){ // Изменение скорости motor1.setSpeed(spdL); motor2.setSpeed(spdR); motor3.setSpeed(spdL); motor4.setSpeed(spdR); } int u_distance(){ // Измерение расстояния digitalWrite(TRIGGER_PIN, HIGH); // Подаем сигнал на выход микроконтроллера delayMicroseconds(10); // Удерживаем 10 микросекунд digitalWrite(TRIGGER_PIN, LOW); // Затем убираем microsec = pulseIn(ECHO_PIN, HIGH, 20000); // Замеряем длину импульса distance = microsec/58; // Пересчитываем в сантиметры return distance; // Возвращаем значение } float compass_measure(){ // функция считывания данных с компаса и расчета направления MagnetometerScaled scaled = compass.ReadScaledAxis(); int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis) float heading = atan2(scaled.YAxis, scaled.XAxis); // корректируем значения с учетом знаков if(heading < 0) heading += 2*PI; if(heading > 2*PI) heading -= 2*PI; heading = heading * 180/M_PI; // переводим радианы в градусы return heading; }