// библиотека для работы с платформой Strela #include // библиотека для работы с I2C-расширителем портов #include // EEPROM — энергонезависимая память // библиотека для записи и считывания информации с EEPROM #include // библиотека для работы с сервоприводами #include // создадим объект для управления сервоприводом Servo myservo; // это число мы будем использовать в логике поворотов int defaultSpeed = 100; // w1 и w2 - это скорость вращения первого и второго мотора // скорость регулируется в пределах от -255 до 255 // если это число положительное - мотор будет вращаться вперёд // если отрицательное - назад // если баланс скорости вращения моторов не бы совершен // по умолчанию равны 100 int w1 = 100; int w2 = 100; // значение поправочного коефициента // скорости одного мотора к другому float k = 1; // переменная хранит контрольную сумму // проверка на то, было ли что нибудь записано в EEPROM int sum; // переменные состояния каждой из 4 кнопок // была ли кнопка отпущена? boolean button1WasUp = true; boolean button2WasUp = true; boolean button3WasUp = true; boolean button4WasUp = true; void setup() { // я неправильно прикрутил один мотор // поэтому, чтобы их не перекручивать // можно воспользоваться этой функцией. // направление вращения мотора 2 будет изменено. motorConnection(0, 1); // открываем последовательный порт со скоростью 9600 бод Serial.begin(9600); // Bluetooth Bee по умолчанию использует скорость 9600 бод Serial1.begin(9600); // пикнем зуммером с частотой 1000 Гц, 100 мс tone(BUZZER, 1000, 100); delay(500); // считываем значение из 4 ячейки памяти EEPROM sum = EEPROMReadInt(4); // записывали ли мы в EEPROM значение баланса скоростей if (sum == 777) { // чтение из памяти значение баланса скоростей w1 = EEPROMReadInt(0); w2 = EEPROMReadInt(2); } delay(100); // нажата ли кнопка S1 // вход в меню настройки баланса скорости моторов if (uDigitalRead(S1)) { // пищим 3 раза зуммером tone(BUZZER, 500, 50); delay(300); tone(BUZZER, 500, 50); delay(300); tone(BUZZER, 500, 50); delay(300); // вызываем функцию баланса скорости моторов balanceMotors(); } // зажгём первый и четвёртый светодиод uDigitalWrite(L1, HIGH); uDigitalWrite(L4, HIGH); // вызываем функцию нахождение поправочного коефициента // скорости одного мотора к другому correction(); } void loop() { // если появились новые команды // вызываем функцию управления if (Serial1.available() > 0) { control(); } // вывод скоростей serialPrint(); } // функция настройки баланса скорости моторов void balanceMotors() { while (1) { // зажгём второй и третий светодиод uDigitalWrite(L2, HIGH); uDigitalWrite(L3, HIGH); // если левое колесо (мотор 1) медленнее правого (мотор 2) // нам нужно определить клик кнопки // определить момент «клика» несколько сложнее, чем факт того, // что кнопка сейчас просто нажата. Для определения клика мы // сначала понимаем, отпущена ли кнопка прямо сейчас boolean button2IsUp = uDigitalRead(S2); // если кнопка была отпущена и не отпущена сейчас // и значение первого мотора менее 255 if (!button2WasUp && button2IsUp && w1 < 255) { // может это «клик», а может и ложный сигнал (дребезг), // возникающий в момент замыкания/размыкания пластин кнопки, // поэтому даём кнопке полностью «успокоиться» delay(10); // и считываем сигнал снова button2IsUp = uDigitalRead(S2); // если она всё ещё нажата, значит это клик! if (button2IsUp) { // Скорость первого мотора увеличиваем, а второго уменьшаем w1++; w2--; } } // запоминаем последнее состояние кнопки для новой итерации button2WasUp = button2IsUp; // если правое колесо (мотор 2) медленнее левого (мотор 1) // нам нужно определить клик кнопки // определить момент «клика» несколько сложнее, чем факт того, // что кнопка сейчас просто нажата. Для определения клика мы // сначала понимаем, отпущена ли кнопка прямо сейчас boolean button4IsUp = uDigitalRead(S4); // если кнопка была отпущена и не отпущена сейчас // и значение второго мотора менее 255 if (!button4WasUp && button4IsUp && w2 < 255) { // может это «клик», а может и ложный сигнал (дребезг), // возникающий в момент замыкания/размыкания пластин кнопки, // поэтому даём кнопке полностью «успокоиться» delay(10); // и считываем сигнал снова button4IsUp = uDigitalRead(S4); // если она всё ещё нажата, значит это клик! if (button4IsUp) { // Скорость второго мотора увеличиваем, а первого уменьшаем w1--; w2++; } } // запоминаем последнее состояние кнопки для новой итерации button4WasUp = button4IsUp; // Индикация увеличение скорости первого мотора if (uDigitalRead(S2)) { uDigitalWrite(L4, HIGH); } else { uDigitalWrite(L4, LOW); } // Индикация увеличение скорости второго мотора if (uDigitalRead(S4)) { uDigitalWrite(L1, HIGH); } else { uDigitalWrite(L1, LOW); } // вывод скоростей serialPrint(); // ход по значениям скоростей w1 и w2 drive(w1, w2); // если нажата кнопка S1 // пишем CANCEL в Serial // и выходим из бесконечного цикла while(1) без сохранения if (!button1WasUp && uDigitalRead(S1)) { Serial.println("CANCEL"); break; } button1WasUp = uDigitalRead(S1); // если нажата кнопка S3 if (!button3WasUp && uDigitalRead(S3)) { // сохраняем значение первого мотора EEPROMWriteInt(0, w1); // сохраняем значение второго мотора EEPROMWriteInt(2, w2); // сохраняем значение контрольной суммы EEPROMWriteInt(4, 777); // Пишем SAVE в Serial и выходим из бесконечно цикла Serial.println("SAVE"); break; } button3WasUp = uDigitalRead(S3); } /// while (1) // останавливаем моторы drive(0, 0); // погасим второй и третий светодиод uDigitalWrite(L2, LOW); uDigitalWrite(L3, LOW); // пикнем 3 раза зуммером tone(BUZZER, 1000, 50); delay(100); tone(BUZZER, 1000, 50); delay(100); tone(BUZZER, 1000, 50); delay(100); } /// balanceMotors //запись двухбайтового числа в память void EEPROMWriteInt(int address, int value) { EEPROM.write(address, lowByte(value)); EEPROM.write(address + 1, highByte(value)); } //чтение двухбайтового из числа из памяти unsigned int EEPROMReadInt(int address) { byte lowByte = EEPROM.read(address); byte highByte = EEPROM.read(address + 1); return (highByte << 8) | lowByte; } void serialPrint() { Serial.print("speed w1 = "); Serial.print(w1); Serial.print(" "); Serial.print("speed w2 = "); Serial.println(w2); } // пуск сервопривода постоянного вращения void servoStart(void) { myservo.attach(11); myservo.write(0); } // остановка сервопривода void servoStop(void) { // Самый простой способ остановить серву постоянного вращения // отсоединиться от неё myservo.detach(); } void control() // функция управления { // считаем значение пришедшей команды char dataIn = Serial1.read(); if (dataIn == 'F') { // пришла команда "F", едем вперёд drive(w1, w2); } else if (dataIn == 'B') { // пришла команда "B", едем назад drive(-w1, -w2); } else if (dataIn == 'L') { // пришла команда "L", поворачиваем налево на месте drive(-w1, w2); } else if (dataIn == 'R') { // пришла команда "R", поворачиваем направо на месте drive(w1, -w2); } else if (dataIn == 'I') { // пришла команда "I", едем вперёд и направо drive(defaultSpeed + w1, defaultSpeed - w2); } else if (dataIn == 'J') { // пришла команда "J", едем назад и направо drive(-defaultSpeed - w1, -defaultSpeed + w2); } else if (dataIn == 'G') { // пришла команда "G", едем вперёд и налево drive(defaultSpeed - w1, defaultSpeed + w2); } else if (dataIn == 'H') { // пришла команда "H", едем назад и налево drive(-defaultSpeed + w1, -defaultSpeed - w2); } else if (dataIn == 'S') { // если пришла команда "S", стоим на месте drive(0, 0); } else if (dataIn == 'X') { // пришла команда "X", крутим серву servoStart(); } else if (dataIn == 'x') { // пришла команда "x", останавливаем серву servoStop(); } else if (dataIn == 'V') { // пришла команда "V", пищим tone(BUZZER, 1000); } else if (dataIn == 'v') { // пришла команда "v", не пищим noTone(BUZZER); } else if (((dataIn - '0') >= 0) && ((dataIn - '0') <= 9)) { // настройка скорости вращения обоих моторов от 0 до 9 // если первый мотор быстрее if (w1 > w2) { // сохраняем новое значение скорости обоих моторов // второй с поправкой на баланс w1 = (dataIn - '0') * 25; w2 = (dataIn - '0') * 25*k; } else { // сохраняем новое значение скорости обоих моторов // первый с поправкой на баланс w1 = (dataIn - '0') * 25*k; w2 = (dataIn - '0') * 25; } } else if (dataIn == 'q') { // если "q" - полный газ if (w1 > w2) { // первый мотор максимум, второй с поправкой на баланс w1 = 255; w2 = 255*k; } else { // второй мотор максимум, первый с поправкой на баланс w1 = 255*k; w2 = 255; } } } /// end control // функция нахождения поправочного коефициента // скорости одного мотора к другому void correction() { float m1 = w1; float m2 = w2; if (m1 > m2) { k = m2 / m1; } else { k = m1 / m2; } }