Убрал компас, вместо него приклеил серву на которую впоследствии будет установлен ультразвуковой дальномер, а пока приклеил вместо него веселую рожицу.
Подключил пьезо-пищалку и запрограммировал ее воспроизводить мелодию.
Подправил код чтобы серва поворачивалась не так быстро и чтобы задержек не было длинных. Сначала мелодия игралась медленно, я ее чуть ускорил и поставил вместо задержки когда робот отъезжает от препятствия.
Подключил 8 ногу драйвера к +5, в результате робот начал носиться значительно быстрее почти в 6 раз. На видео видно что по сравнению с предыдущими разами он поворачивается на угол в 6 раз больше, соответственно нужно будет уменьшить задержки почти в 6 раз.
Вот само видео:
На данный момент код выглядит так:
// Определяем какие пины за что отвечают
include
define LM1 7 // Левый мотор 1 контакт
define LM2 6 // Левый мотор 2 контакт
define RM1 5 // Правый мотор 1 контакт
define RM2 4 // Правый мотор 2 контакт
define BUTTON_PIN 2 // кнопка пуска
define LED_PIN 13
define SENS_LEFT 8 // левый сенсор 8 контакт
define SENS_RIGH 9 // правый сенсор
boolean buttonWasUp = true; // была ли кнопка отпущена?
boolean workEnabled = false; // работают ли движки?
boolean ledEnabled = false; // включен ли свет?
boolean r = false; // поворот головы вправо
int angle = 45; // установка первоначального угла
float ugol;
Servo myServo;
int speakerPin = 10;
int length = 15; // число нот
char notes[] = "ccggaagffeeddc "; // пробел - пауза
int beats[] = { 1, 1, 1, 1, 1, 1, 2, 1, 1, 1, 1, 1, 1, 2, 4 };
int tempo = 75;
void playTone(int tone, int duration) {
for (long i = 0; i < duration * 1000L; i += tone * 2) {
digitalWrite(speakerPin, HIGH);
delayMicroseconds(tone);
digitalWrite(speakerPin, LOW);
delayMicroseconds(tone);
}
}
void playNote(char note, int duration) {
char names[] = { 'c', 'd', 'e', 'f', 'g', 'a', 'b', 'C' };
int tones[] = { 1915, 1700, 1519, 1432, 1275, 1136, 1014, 956 };
// проиграть тон, соответствующий ноте
for (int i = 0; i < 8; i++) {
if (names[i] == note) {
playTone(tones[i], duration);
}
}
}
void myMelody() {
for (int i = 0; i < length; i++) {
if (notes[i] == ' ') {
delay(beats[i] * tempo); // пауза
} else {
playNote(notes[i], beats[i] * tempo);
}
// пауза между нотами
delay(tempo / 2);
}
}
void setup() {
pinMode(LM1, OUTPUT);
pinMode(LM2, OUTPUT);
pinMode(RM1, OUTPUT);
pinMode(RM2, OUTPUT);
pinMode(LED_PIN, OUTPUT);
pinMode(BUTTON_PIN, INPUT_PULLUP);
pinMode(SENS_LEFT, INPUT_PULLUP);
pinMode(SENS_RIGH, INPUT_PULLUP);
Serial.begin(9600);
myServo.attach(11);
pinMode(speakerPin, OUTPUT);
}
//------------------STOP---------------------
void stop_motors() // Функция остановки моторов
{
digitalWrite(LM1, LOW);
digitalWrite(LM2, LOW);
digitalWrite(RM1, LOW);
digitalWrite(RM2, LOW);
}
//---------------FORWARD-----------------------
void forward() // Функция движения ВПЕРЁД
{
digitalWrite(LM1, LOW);
digitalWrite(LM2, HIGH);
digitalWrite(RM1, HIGH);
digitalWrite(RM2, LOW);
}
//----------------BACK------------------------
void back() // Функция движения НАЗАД
{
digitalWrite(LM1, HIGH);
digitalWrite(LM2, LOW);
digitalWrite(RM1, LOW);
digitalWrite(RM2, HIGH);
}
//----------------LEFT------------------------
void left() // Функция поворота ВЛЕВО
{
digitalWrite(LM1, HIGH);
digitalWrite(LM2, LOW);
digitalWrite(RM1, HIGH);
digitalWrite(RM2, LOW);
}
//----------------RIGHT------------------------
void right() //Функция поворота ВПРАВО
{
digitalWrite(LM1, LOW);
digitalWrite(LM2, HIGH);
digitalWrite(RM1, LOW);
digitalWrite(RM2, HIGH);
}
//-------------------------------------------
void loop()
{
// определить момент «клика» несколько сложнее, чем факт того,
// что кнопка сейчас просто нажата. Для определения клика мы
// сначала понимаем, отпущена ли кнопка прямо сейчас...
boolean buttonIsUp = digitalRead(BUTTON_PIN);
// ...если «кнопка была отпущена и (&&) не отпущена сейчас»...
if (buttonWasUp && !buttonIsUp) {
// ...может это «клик», а может и ложный сигнал (дребезг),
// возникающий в момент замыкания/размыкания пластин кнопки,
// поэтому даём кнопке полностью «успокоиться»...
delay(10);
// ...и считываем сигнал снова
buttonIsUp = digitalRead(BUTTON_PIN);
if (!buttonIsUp) { // если она всё ещё нажата...
// ...это клик! Переворачиваем переменную работы
workEnabled = !workEnabled;
ledEnabled = !ledEnabled;
digitalWrite(LED_PIN, ledEnabled);
}
}
// запоминаем последнее состояние кнопки для новой итерации
buttonWasUp = buttonIsUp;
if (!workEnabled) {
stop_motors(); // СТОП
}
if (workEnabled) {
myServo.write(angle);
if (r) {
ugol = ugol + 0.5;
angle = ugol;
if (angle >= 170) {
r = !r;
}
}
else
{
ugol = ugol - 0.5;
angle = ugol;
if (angle <= 50) {
r = !r;
}
}
Serial.println(ugol);
boolean leftSens = digitalRead(SENS_LEFT);
boolean righSens = digitalRead(SENS_RIGH);
forward(); // ВПЕРЁД
if (!leftSens && righSens) {
back (); // НАЗАД
myMelody();
// delay(1000);
left(); // В ЛЕВО
delay(1000);
forward(); // ВПЕРЁД
}
if (leftSens && !righSens) {
back (); // НАЗАД
myMelody();
// delay(1000);
right(); // В ПРАВО
delay(1000);
forward(); // ВПЕРЁД
}
if (!leftSens && !righSens) {
back (); // НАЗАД
myMelody();
// delay(1000);
right(); // В ПРАВО
delay(2000);
forward(); // ВПЕРЁД
}
}
}