TP_LINK MR3020 + Arduino Nano, обмен данными по USB, проблемы и решения
В контроллере Arduino Nano не слишком много памяти для сложных программ, например, достаточно трудно заставить Ардуинку "разговаривать", что намного проще сделать при помощи Linux системы. Там же можно вести всю сложную обработку с датчиков, а управление на низком уровне оставить за контроллером. Единственное, что необходимо выбрать мобильный вариант Linux устройства, например TP_LINK MR3020, компактный недорогой роутер, который можно прошить при помощи OpenWrt или CyberWrt и довольно просто получить Linux с USB и WiFi на борту, если подключить USB звуковую карту, то будет вообще интересный вариант.
Идея сделать робота, который управляется при помощи Arduino Nano в качестве первичного контроллера (управление движением, считывание датчиков) а MR3020 несет на себе всю логику работы, включая "проговаривание" звуковых файлов и трансляцию видео.
Как потом оказалось, TP_LINK MR3020 и Arduino не слишком хорошо дружат между собой, и возникает некоторое количество проблем, если попытаться подключить именно по USB. Для начала нужно прошить устройство прошивкой CyberWrt http://cyber-place.ru/showthread.php?t=720
Затем установить драйверы USB порта. Для Arduino Nano нужны драйверы ftdi
root@CyberWrt:~# opkg list-installed kmod*
kmod-ath - 3.10.4+2013-06-27-1
kmod-ath9k - 3.10.4+2013-06-27-1
kmod-ath9k-common - 3.10.4+2013-06-27-1
kmod-cfg80211 - 3.10.4+2013-06-27-1
kmod-crypto-aes - 3.10.4-1
kmod-crypto-arc4 - 3.10.4-1
kmod-crypto-core - 3.10.4-1
kmod-crypto-hash - 3.10.4-1
kmod-crypto-manager - 3.10.4-1
kmod-fs-ext4 - 3.10.4-1
kmod-fs-ntfs - 3.10.4-1
kmod-fs-vfat - 3.10.4-1
kmod-fuse - 3.10.4-1
kmod-gpio-button-hotplug - 3.10.4-1
kmod-hid - 3.10.4-1
kmod-hid-generic - 3.10.4-1
kmod-i2c-core - 3.10.4-1
kmod-input-core - 3.10.4-1
kmod-input-evdev - 3.10.4-1
kmod-ipt-conntrack - 3.10.4-1
kmod-ipt-conntrack-extra - 3.10.4-1
kmod-ipt-core - 3.10.4-1
kmod-ipt-ipopt - 3.10.4-1
kmod-leds-gpio - 3.10.4-1
kmod-ledtrig-default-on - 3.10.4-1
kmod-ledtrig-netdev - 3.10.4-1
kmod-ledtrig-timer - 3.10.4-1
kmod-lib-crc-ccitt - 3.10.4-1
kmod-lib-crc16 - 3.10.4-1
kmod-mac80211 - 3.10.4+2013-06-27-1
kmod-nls-base - 3.10.4-1
kmod-nls-cp437 - 3.10.4-1
kmod-nls-iso8859-1 - 3.10.4-1
kmod-scsi-core - 3.10.4-1
kmod-sound-core - 3.10.4-1
kmod-usb-audio - 3.10.4-1
kmod-usb-core - 3.10.4-1
kmod-usb-hid - 3.10.4-1
kmod-usb-net - 3.10.4-1
kmod-usb-net-cdc-ether - 3.10.4-1
kmod-usb-ohci - 3.10.4-1
kmod-usb-serial - 3.10.4-1
kmod-usb-serial-ftdi - 3.10.4-1
kmod-usb-serial-option - 3.10.4-1
kmod-usb-serial-wwan - 3.10.4-1
kmod-usb-storage - 3.10.4-1
kmod-usb-uhci - 3.10.4-1
kmod-usb2 - 3.10.4-1
kmod-video-core - 3.10.4-1
kmod-video-uvc - 3.10.4-1
kmod-video-videobuf2 - 3.10.4-1
root@CyberWrt:~# opkg list kmod*
kmod-ath - 3.10.4+2013-06-27-1
kmod-ath9k - 3.10.4+2013-06-27-1
kmod-ath9k-common - 3.10.4+2013-06-27-1
kmod-cfg80211 - 3.10.4+2013-06-27-1
kmod-crypto-aes - 3.10.4-1
kmod-crypto-arc4 - 3.10.4-1
kmod-crypto-core - 3.10.4-1
kmod-crypto-hash - 3.10.4-1
kmod-crypto-manager - 3.10.4-1
kmod-fs-ext4 - 3.10.4-1
kmod-fs-ntfs - 3.10.4-1
kmod-fs-vfat - 3.10.4-1
kmod-fuse - 3.10.4-1
kmod-gpio-button-hotplug - 3.10.4-1
kmod-hid - 3.10.4-1
kmod-hid-generic - 3.10.4-1
kmod-i2c-core - 3.10.4-1
kmod-input-core - 3.10.4-1
kmod-input-evdev - 3.10.4-1
kmod-ipt-conntrack - 3.10.4-1
kmod-ipt-conntrack-extra - 3.10.4-1
kmod-ipt-core - 3.10.4-1
kmod-ipt-ipopt - 3.10.4-1
kmod-leds-gpio - 3.10.4-1
kmod-ledtrig-default-on - 3.10.4-1
kmod-ledtrig-netdev - 3.10.4-1
kmod-ledtrig-timer - 3.10.4-1
kmod-lib-crc-ccitt - 3.10.4-1
kmod-lib-crc16 - 3.10.4-1
kmod-mac80211 - 3.10.4+2013-06-27-1
kmod-nls-base - 3.10.4-1
kmod-nls-cp437 - 3.10.4-1
kmod-nls-iso8859-1 - 3.10.4-1
kmod-scsi-core - 3.10.4-1
kmod-sound-core - 3.10.4-1
kmod-usb-audio - 3.10.4-1
kmod-usb-core - 3.10.4-1
kmod-usb-hid - 3.10.4-1
kmod-usb-net - 3.10.4-1
kmod-usb-net-cdc-ether - 3.10.4-1
kmod-usb-ohci - 3.10.4-1
kmod-usb-serial - 3.10.4-1
kmod-usb-serial-ftdi - 3.10.4-1
kmod-usb-serial-option - 3.10.4-1
kmod-usb-serial-wwan - 3.10.4-1
kmod-usb-storage - 3.10.4-1
kmod-usb-uhci - 3.10.4-1
kmod-usb2 - 3.10.4-1
kmod-video-core - 3.10.4-1
kmod-video-uvc - 3.10.4-1
kmod-video-videobuf2 - 3.10.4-1
Насчет физического подключения. У MR3020 всего один порт USB, самое простое решение, которое приходит в голову - поставить USB Hub и через него уже работать. Первая проблема может возникнуть в питании Arduino, которое логично проводить через тот же USB. Померил напряжение, на роутер поступает 4,5 вольта (в моем случае). После хаба Ардуинке достается 4, падает как на хабе, так и на USB кабеле, так что кабель нужно выбирать правильный, короткий и с толстыми проводами, сам блок питания может давать на совсем стабильное напряжение. Если роутер нормально работает и от 3,5 вольт, то для Arduino такое низкое напряжение питания может быть критично, особенно, если подключены дополнительные потребители по питанию, которые дают просадку напряжения, например, при старте моторов на одном из блоков питания у меня Ардуинка просто перегружается. USB hub с дополнительным питанием, эту проблему решает, особенно, если как дополнительное питание подключена отдельная батарея. В итоге я использовал две батареи - одна для двигателей, а другая для питания роутера и Arduino.
Следующая проблема в том, что FT232 USB-Serial (UART) на Arduino - USB 1.1, а роутер не может работать с USB 1.1, ему обязательно нужны устройства USB 2.0 High-speed, иначе - не видит, проблему решает опять же USB Hub, который должен обязательно поддерживать High-speed.
root@CyberWrt:~# lsusb
Bus 001 Device 002: ID 1a40:0201 Terminus Technology Inc. FE 2.1 7-port Hub
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 001 Device 003: ID 0403:6001 Future Technology Devices International, Ltd FT232 USB-Serial (UART) IC
Bus 001 Device 004: ID 0930:6545 Toshiba Corp. Kingston DataTraveler 102 Flash Drive / HEMA Flash Drive 2 GB / PNY Attache 4GB Stick
После установки драйверов и подключения Arduino через хаб, появляется устройство /dev/ttyUSB0
root@CyberWrt:~# ls /dev
bus mtd1 mtdblock0 ptmx ttyS1 ttyS5
console mtd1ro mtdblock1 pts ttyS10 ttyS6
cpu_dma_latency mtd2 mtdblock2 random ttyS11 ttyS7
full mtd2ro mtdblock3 sda ttyS12 ttyS8
fuse mtd3 mtdblock4 sda1 ttyS13 ttyS9
kmsg mtd3ro mtdblock5 shm ttyS14 ttyUSB0
log mtd4 network_latency snd ttyS15 urandom
mem mtd4ro network_throughput tty ttyS2 watchdog
mtd0 mtd5 null ttyATH0 ttyS3 zero
mtd0ro mtd5ro port ttyS0 ttyS4
root@CyberWrt:/www1# dmesg
[ 19.700000] ohci_hcd: USB 1.1 'Open' Host Controller (OHCI) Driver
[ 19.720000] i2c /dev entries driver
[ 19.760000] usbcore: registered new interface driver snd-usb-audio
[ 19.800000] usbcore: registered new interface driver usbserial
[ 19.800000] usbcore: registered new interface driver usbserial_generic
[ 19.820000] usbserial: USB Serial support registered for generic
[ 19.860000] Linux video capture interface: v2.00
[ 19.900000] hidraw: raw HID events driver (C) Jiri Kosina
[ 19.910000] usbcore: registered new interface driver cdc_ether
[ 19.950000] usbcore: registered new interface driver ftdi_sio
[ 19.950000] usbserial: USB Serial support registered for FTDI USB Serial Device
[ 19.960000] ftdi_sio 1-1.1:1.0: FTDI USB Serial Device converter detected
[ 19.980000] usb 1-1.1: Detected FT232RL
[ 19.980000] usb 1-1.1: Number of endpoints 2
[ 19.980000] usb 1-1.1: Endpoint 1 MaxPacketSize 16384
[ 19.990000] usb 1-1.1: Endpoint 2 MaxPacketSize 16384
[ 19.990000] usb 1-1.1: Setting MaxPacketSize 64
[ 20.010000] usb 1-1.1: FTDI USB Serial Device converter now attached to ttyUSB0
[ 20.030000] usbcore: registered new interface driver option
[ 20.030000] usbserial: USB Serial support registered for GSM modem (1-port)
[ 20.090000] usbcore: registered new interface driver usbhid
[ 20.090000] usbhid: USB HID core driver
Может быть проблема с зависанием порта на отдельных роутерах, которая связана с некорректной работой WiFi + USB, подробно обсуждается здесь
И есть патч для OpenWrt версии 39212, но поскольку CyberWrt у нас версии r37816, то патч не подойдет.
root@CyberWrt:~# cat /etc/openwrt_release
DISTRIB_ID="OpenWrt"
DISTRIB_RELEASE="Bleeding Edge"
DISTRIB_REVISION="r37816"
DISTRIB_CODENAME="barrier_breaker"
DISTRIB_TARGET="ar71xx/generic"
DISTRIB_DESCRIPTION="OpenWrt Barrier Breaker r37816"
У меня был проблема - периодическое отваливание USB порта. Выглядит это как кратковременная потеря связи. У меня длительность такой потери 1-2 секунды. В протоколе dmesg видим в момент потери связи вот такие сообщения
[ 3688.860000] ftdi_sio ttyUSB0: CSIZE was set but not CS7-CS8
По одному сообщению каждый раз, когда связь с портом теряется. При этом порт остается открытым, но слетают настройки.
Если после этого не восстановить стандартные значения порта, со связь будет потеряна совсем. Оказалось, что когда устанавливал на роутер все подряд для теста, установил и модуль Терморегулятор-USB, который запускался при старте системы и периодически изменял настройки порта. Если удалить все лишнее и к порту никто дополнительно не обращается, то работает стабильно.
Сейчас на голове робота установлены три датчика: левый и правый фотодиоды для отслеживания изменения освещения и эхолот для отслеживания расстояния до препятствий. Голова может поворачиваться влево и вправо для проверки расстояния до препятствий слева и справа. Простая программа на PHP работает по следующему алгоритму:
Робот поворачивает голову в положение 20, 45 и 90 градусов и производит пять замеров датчиков в каждом положении.
root@CyberWrt:/www1# php-cgi test_a.php
X-Powered-By: PHP/5.4.19
Content-type: text/html
Ready
Port opened
0 SP:20 T2:31.21 Way:0 FL:526 FR:409 FF:11
0 SP:20 T2:31.26 Way:0 FL:527 FR:409 FF:11
0 SP:20 T2:31.26 Way:0 FL:527 FR:409 FF:11
0 SP:20 T2:31.21 Way:0 FL:527 FR:409 FF:11
0 SP:20 T2:31.31 Way:0 FL:527 FR:409 FF:11
0 SP:45 T2:31.31 Way:0 FL:458 FR:415 FF:42
0 SP:45 T2:31.17 Way:0 FL:458 FR:415 FF:42
0 SP:45 T2:31.26 Way:0 FL:458 FR:416 FF:42
0 SP:45 T2:31.17 Way:0 FL:458 FR:416 FF:42
0 SP:45 T2:31.31 Way:0 FL:458 FR:416 FF:41
0 SP:90 T2:31.21 Way:0 FL:177 FR:233 FF:167
0 SP:90 T2:31.31 Way:0 FL:177 FR:233 FF:167
0 SP:90 T2:31.26 Way:0 FL:177 FR:233 FF:165
0 SP:90 T2:31.31 Way:0 FL:177 FR:233 FF:165
0 SP:90 T2:31.21 Way:0 FL:177 FR:232 FF:165
SP - положение головы, T2 - датчик температуры Way - спидометр FL - левый фотодиод FR - правый фотодиод FF - дальномер
текст программ
<?php
// robot.inc
function numberFormat($digit, $width) {
while(strlen($digit) < $width)
$digit = '0' . $digit;
return $digit;
}
Class Robot
{
public $T1=0; // датчик температуры
public $T2=0; // датчик температуры
public $Way=0; // путь
public $CurWay=0; // текущий путь
public $MaxWay=0; // макcимальный путь
public $Z = 0; // ускорение отклонения
public $FL = 0; // левый глаз
public $FR = 0; // правый глаз
public $FF = 0; // радар
public $SP = 90; // градусы, сколько стоит голова
public $port = "/dev/ttyUSB0";
public $fd; // для открытия порта
// инициализация порта
Function InitPort($Port) {
// для ардуинки -hupcl отключает перезагрузку при открытии порта!
$command = "stty -F $Port cs8 9600 ignbrk -brkint -icrnl -imaxbel -opost -onlcr -isig -icanon -iexten -echo -echoe -echok -echoctl -echoke noflsh -ixon -crtscts -hupcl";
print `$command`;
$this->port = $Port;
return;
}
// установили порт по умолчанию для открытия
Function SetPort($Port) {
$this->port=$Port;
}
// открыли порт
Function OpenPort() {
$this->port;
$this->fd = dio_open($this->port, O_RDWR | O_NONBLOCK);
if (!$this->fd){
print "Error open port $port";
$ret = "OPEN PORT $this->port ERR";
return false;
}
return true; // все хорошо, порт открыт
}
Function ClosePort() {
dio_close($this->fd);
}
// записали в порт
Function WriteLn($Param){
$delay = .01; // немножко подождем, перед записью в порт;
$now = microtime(1); // получим время с микросекундами
$end = $now + $delay; // в течение $delay секунды ждем ответа
$result = '';
// printf ("> $now > $end");
while ($now < $end) { // задержка
$now = microtime(1);
}
dio_write($this->fd,"$Param\r"); // теперь запишем
return;
}
Function ReadByte($byte){ // чтение байта
$result=dio_read($this->fd_read,$byte); // считываем сразу, к примеру по 100 байт
return $result;
}
Function Read(){ // чтение строки ответа (одной строки!)
$delay = 2; // если в течение этого времени ответ не получен, то ошибка
$now = microtime(1); // получим время с микросекундами
$end = $now + $delay; // в течение $delay секунды ждем ответа
$result = '';
$buff="";
while ($now < $end) {
$result=dio_read($this->fd_read,1); // считываем сразу, к примеру по 1 байт
//print $result;
$test = ord($result);
// пока ничего не считали, ждем, может придут еще данные
if ($test ==0){
$now = microtime(1);
continue;
}
// конец строки 10 - внутри команды. 13 - завершение группы, типа OK
// но нам важна каждая строка,
if ($test==10){ // это внутри одной группы данных, еще не конец передачи
//print "$buff";
if ($buff==""){ // пустые строки не отдаем
$now = microtime(1);
continue;
}
return $buff;
}
if ($test==13){ // это внутри одной группы данных, еще не конец передачи
//print "$buff";
if ($buff==""){ // пустые строки не отдаем
$now = microtime(1);
continue;
}
return $buff;
}
$buff = "$buff$result";
// print "$test:$buff\n";
$now = microtime(1);
continue;
}
return "TIMEOUT: $buff"; // конец цикла, ничего не получили, возвращаем
}
function SetSP($Param){ // повернуть голову от 0 до 180
$str = numberFormat($Param, 3);
$this->WriteLn("SP:$str#");
$this->SP=$Param;
$ret1 = $this->Read(); // это данные
//print "SP->$ret $ret1\n";
return;
}
function SensorRead(){ // чтение данных сенсоров и запись их во внутренние переменные
$this->WriteLn("TE:010#");
$ret1 = $this->Read(); // это O
$ret = $this->Read(); // это OK
// print ">>$ret1 $ret<<\n";
if (trim($ret)!="OK"){
$ret1=dio_read($this->fd,100);
print " Err:$ret $ret1\n";
}
if ($ret1=="TIMEOUT" || $ret1=="OK"){
$ret2=dio_read($this->fd,100);
print "Err:$ret1 $ret2\n";
}
$ret2 = $this->ReadByte(100); // это OK
//"Way:0 CurWay:0 MaxWay:0 Z:-317.49 T1:39.53 T2:31.31 FL:343 FR:383 FF:35"
// print "$ret1\n";
$mass = explode(' ', $ret1);
$this->Way = substr($mass[0],strrpos($mass[0], ':')+1);
$this->CurWay = substr($mass[1],strrpos($mass[1], ':')+1);
$this->MaxWay = substr($mass[2],strrpos($mass[2], ':')+1);
$this->Z = substr($mass[3],strrpos($mass[3], ':')+1);
$this->T1 = substr($mass[4],strrpos($mass[4], ':')+1);
$this->T2 = substr($mass[5],strrpos($mass[5], ':')+1);
$this->FL = substr($mass[6],strrpos($mass[6], ':')+1);
$this->FR = substr($mass[7],strrpos($mass[7], ':')+1);
$this->FF = substr($mass[8],strrpos($mass[8], ':')+1);
return;
}
}
?>
--------------------------------
<?php
include "robot.inc";
set_time_limit(2000); // время выполнения не больше 2000 секунд
print "Ready\n";
$robot = new Robot();
$robot->InitPort("/dev/ttyUSB0");
if (!$robot->OpenPort()){
print "Error open port $robot->port\n";
return;
}
print $robot->ReadByte(100); // if something else
$robot->InitPort("/dev/ttyUSB0"); // remove reboot setup key
print "Port opened\n";
$ret="";
$i=0; // 10 sec
while (true){
$robot->SetSP(20);
sleep(1);
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n\n";
$robot->SetSP(45);
sleep(1);
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n\n";
$robot->SetSP(90);
sleep(1);
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n";
$robot->SensorRead();
print "$i SP:$robot->SP T2:$robot->T2 Way:$robot->Way FL:$robot->FL FR:$robot->FR FF:$robot->FF \n\n";
$i++;
if ($i>300){
print "End\n";
$i=0;
$robot->ClosePort();
return;
}
$robot->ClosePort();
}
?>
---------------------------
robot.ino скетч для Ардуино
/*
*/
#include <Servo.h>
#include <Wire.h>
#include "Kalman.h"
/* аналоговые входы */
#define tempPin 0
#define flPin 2
#define frPin 3
// wired connections
#define HG7881_B_IA 5 // D7 --> Motor B Input A --> MOTOR B +
#define HG7881_B_IB 4 // D6 --> Motor B Input B --> MOTOR B -
// wired connections
#define HG7881_A_IA 6 // D5 --> Motor A Input A --> MOTOR B +
#define HG7881_A_IB 7 // D4 --> Motor A Input B --> MOTOR B -
// functional connections
#define MOTOR_B_PWM HG7881_B_IA // Motor B PWM Speed
#define MOTOR_B_DIR HG7881_B_IB // Motor B Direction
// functional connections
#define MOTOR_A_PWM HG7881_A_IA // Motor B PWM Speed
#define MOTOR_A_DIR HG7881_A_IB // Motor B Direction
// the actual values for "fast" and "slow" depend on the motor
#define PWM_SLOW 50 // arbitrary slow speed PWM duty cycle
#define PWM_FAST 200 // arbitrary fast speed PWM duty cycle
#define DIR_DELAY 50 // brief delay for abrupt motor changes
#define STOP 0
#define FORWARD 1
#define REVERSE 2
#define LEFT 3
#define RIGHT 4
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;
uint8_t IMUAddress = 0x68;
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
/*
#define RFPin 6
#define LBPin 7
#define LFPin 5
#define RBPin 4
*/
int LBPin = 7;
int LFPin = 6;
int RFPin = 5;
int RBPin = 4;
int Movement = 0 ;// 0 - стоим 1 - вперед 2 - назад 3 - поворот налево, 4 - поворот направо
int LastMovement = 0; // запомнить последнее движение
int Speed=255;
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double accZangle;
double temp;
int cm;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double gyroZangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;
double kalAngleZ;
double GYRO_ANGLE_Z;
double FFTime;
unsigned int Way=0; // пройденный путь
unsigned int CurWay=0; // прошли с последнего сброса
signed int MaxWay=-100; // -1 не учитывать, если больше, то ехать, если 0, то стоять должны пройти, если меньше, то стоять
uint32_t timer;
// цифровые
int servoPin = 10;
//int servoPin = 3;
//Пин пwww.new.doktor35.ru одключен к ST_CP входу 74HC595
int latchPin = 8;
int trigPin = 9;
int echoPin = 3; //10;
/*Пин подключен к DS входу 74HC595 */
int dataPin = 11;
//Пин подключен к SH_CP входу 74HC595
int clockPin = 12;
Servo myservo;
float temp1;
unsigned int bitsToSend = 0;
bool CC01 = false;
bool CC02 = true; // моргание синими светодиодами
bool FF = true; // опрос дальномера
unsigned long T1 = 0;
unsigned long T2 = 0;
bool leftLED = true; // начать с левого
bool rightLED = false;
int ledcou = 0;
char buff[7];
char incoming_byte;
double GyroTime = 0;
//float Celsius, Fahrenheit, Kelvin;
int sensorValue;
int input_size = 0;
void setup() {
//устанавливаем режим OUTPUT
pinMode(latchPin, OUTPUT);
pinMode(dataPin, OUTPUT);
pinMode(clockPin, OUTPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
registerWrite(11, HIGH);
registerWrite(14, HIGH);
pinMode( MOTOR_B_DIR, OUTPUT );
pinMode( MOTOR_B_PWM, OUTPUT );
digitalWrite( MOTOR_B_DIR, LOW );
digitalWrite( MOTOR_B_PWM, LOW );
pinMode( MOTOR_A_DIR, OUTPUT );
pinMode( MOTOR_A_PWM, OUTPUT );
digitalWrite( MOTOR_A_DIR, LOW );
digitalWrite( MOTOR_A_PWM, LOW );
//pinMode(LFPin, OUTPUT);
//pinMode(RFPin, OUTPUT);
//pinMode(LBPin, OUTPUT);
//pinMode(RBPin, OUTPUT);
myservo.attach(servoPin);
Serial.begin(115200);
Wire.begin();
Serial.begin(9600);
unsigned long T0 = millis();
FFTime = millis() + 500;
T1 = T0 + 1000;
T2 = T0 + 1300;
Serial.println("READY");
int i = 6;
while (i >= 0) {
buff[i] = 0;
i--;
}
registerWrite(0, LOW);
i2cWrite(0x6B, 0x00); // Disable sleep mode
kalmanX.setAngle(180); // Set starting angle
kalmanY.setAngle(180);
kalmanZ.setAngle(180);
timer = micros();
GyroTime = 0;//micros() + 500;
// delay (100);
// registerWrite(11, LOW);
// registerWrite(14, LOW);
//pinMode(2, OUTPUT);
attachInterrupt(0, WayCount ,RISING); // счетчик пути
}
void WayCount(){ // это прерывание
Way++;
CurWay++;
if (MaxWay>0) {
MaxWay--; // обратный счетчик
}
}
void loop() {
/* Update all the values */
uint8_t* data = i2cRead(0x3B, 14);
accX = ((data[0] << 8) | data[1]);
accY = ((data[2] << 8) | data[3]);
accZ = ((data[4] << 8) | data[5]);
tempRaw = ((data[6] << 8) | data[7]);
gyroX = ((data[8] << 8) | data[9]);
gyroY = ((data[10] << 8) | data[11]);
gyroZ = ((data[12] << 8) | data[13]);
/* Calculate the angls based on the different sensors and algorithm */
accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
accZangle = (atan2(accY,accX)+PI)*RAD_TO_DEG;
//CompAngleZ = (0.6*(CompAngleZ-(gyroZrate)*2/1000))-(0.4*(accZangle));
double gyroXrate = (double)gyroX / 131.0;
double gyroYrate = -((double)gyroY / 131.0);
double gyroZrate = (double)gyroZ / 131.0;
gyroXangle += kalmanX.getRate() * ((double)(micros() - timer) / 1000000); // Calculate gyro angle using the unbiased rate
gyroYangle += kalmanY.getRate() * ((double)(micros() - timer) / 1000000);
gyroZangle += kalmanZ.getRate() * ((double)(micros() - timer) / 1000000);
GYRO_ANGLE_Z = (0.6*(GYRO_ANGLE_Z-(gyroZrate)*2/1000))-(0.4*(accZangle));
double compAngleZ = (0.93*(compAngleZ+(gyroZrate*(double)(micros()-timer)/1000000)))+(0.07*accZangle);
//Filtre kalman
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
kalAngleZ = kalmanZ.getAngle(accZangle, gyroZrate, (double)(micros()-timer)/1000000);
//GYRO_ANGLE_Z = GYRO_ANGLE_Z + kalAngleZ / 22;
timer = micros();
//delay (200);
if (false/*GyroTime < millis()*/){
Serial.print("kalAngleX:");
Serial.print((double)kalAngleX, 0);
Serial.print(" accZ:");
Serial.print((double)accZ, 0);
Serial.print(" gyroZ:");
Serial.print((double)gyroZ, 0);
//Serial.print(gyroX);
Serial.print(" gyroZrate:");
Serial.print(gyroZrate, 0);
Serial.print(" ");
Serial.print("accZangle:");
Serial.print(accZangle, 0);
//Serial.print(gyroY);
Serial.print(" kalAngleZ:");
Serial.print(kalAngleZ, 0);
Serial.print(" GYRO_ANGLE_Z:");
Serial.print(GYRO_ANGLE_Z, 0);
Serial.println(" ");
if (bitRead(bitsToSend, 11) == 1) {
registerWrite(11, LOW);
} else {
registerWrite(11, HIGH);
}
if (bitRead(bitsToSend, 14) == 1) {
registerWrite(14, LOW);
} else {
registerWrite(14, HIGH);
}
GyroTime = 0;//millis() + 300;
}
// The accelerometer's maximum samples rate is 1kHz
if (FF && FFTime < millis()) {
// проверить расстояние
TestDistance(); // там записывается переменная cm
ShowFF(); // отобразить на шкале что получилось
FFTime = millis() + 330;
}
if (CC02) {
FuncCC02();
}
if (CC01) {
// проходим циклом по всем 16 выходам двух регистров
for (int thisLed = 0; thisLed < 16; thisLed++) {
// записываем сигнал в регистр для очередного светодиода
registerWrite(thisLed, HIGH);
// если это не первый светодиод, то отключаем предыдущий
if (thisLed > 0) {
registerWrite(thisLed - 1, LOW);
}
// если это первый светодиод, то отключаем последний
else {
registerWrite(15, LOW);
}
// делаем паузу перед следующией итерацией
delay(100);
}
}
// обработка пришедших команд
while (Serial.available() > 0) {
incoming_byte = Serial.read(); // читать входящее
if (incoming_byte == '\n') {
continue;
}
if (incoming_byte == '\r') {
continue;
}
if (incoming_byte == '#' || input_size==6) { // пришло сообщение, должно быть 6 байт от 0 до 5, обрабатываем и заканчиваться на #
if (input_size != 6 || incoming_byte != '#' ) { // сообщение меньше шести символов или нет завершения, то что-то не так, обнуляем
ClearBuff(); // вызов обнуления
Serial.println("ERR");
continue;
}
// сообщение принято, повторять не нужно,далее пошла обработка
//Serial.println("OK");
// трансляция
if (buff[0] == 'S' && buff[1] == 'P') {
// SP:000 - 090 - 180
int grad = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48) + 10; // поправка ;
if (grad < 10) grad = 10;
if (grad > 180) grad = 180;
myservo.write(grad); // устанавливаем сервопривод в серединное положение
Serial.println("OK");
}
if (buff[0] == 'F' && buff[1] == 'F') { //FF01:1 дальномер
if (buff[5] == '1') {
FF = true;
} else {
FF = false;
}
Serial.println("OK");
}
if (buff[0] == 'C' && buff[1] == 'C' && buff[2] == '0' && buff[3] == '1') {
if (buff[5] == '1') CC01 = true; // запуск теста светодиодов
if (buff[5] == '0') {
CC01 = false;
bitsToSend = 0;
registerWrite(0, LOW);
// запуск теста светодиодов
}
}
if (buff[0] == 'C' && buff[1] == 'C' && buff[2] == '0' && buff[3] == '2') {
CC02 = true; // запуск теста светодиодов
T1 = millis() + 1000; // пауза в секунду
T2 = millis() + 1100; // гореть одну десятую
if (buff[5] == '0') {
CC02 = false;
}
}
// установить длину движения по щелчкам
// MW:999 не более 999 щелчков
if (buff[0] == 'M' && buff[1] == 'W') {
if (buff[3]=='-'){ // типа мы сбрасываем
MaxWay = -100;
} else {
MaxWay = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
}
Serial.println(" OK");
}
// оба мотора вперед
if (buff[0] == 'M' && buff[1] == 'F') {
Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
Serial.println(" OK");
if (Speed > 0 ) {
Forward(Speed);
Movement = FORWARD;
} else {
Movement = STOP;
MaxWay = -100;
Stop();
}
}
// оба мотора назад
if (buff[0] == 'M' && buff[1] == 'B') {
Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
Serial.println(" OK");
if (Speed>0){
Reverse(Speed); // здесь нет установки Movement, есть на автоматическом заднем ходу
}
}
// влево, правый вперед, левый назад
if (buff[0] == 'M' && buff[1] == 'L') { // левый двигатель
Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
Serial.println(" OK");
if (Speed>0){
Movement = LEFT;
Left(Speed);
} else {
Stop();
Movement = STOP;
MaxWay = -100;
}
}
// влево, правый вперед, левый назад
if (buff[0] == 'M' && buff[1] == 'R') { // левый двигатель
Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
Serial.println(" OK");
if (Speed>0) {
Right(Speed);
Movement = RIGHT;
} else {
Stop();
Movement = STOP;
MaxWay = -100;
}
}
if (buff[0] == 'L' && buff[1] == 'F') { // левый двигатель
int Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
digitalWrite( MOTOR_A_DIR, LOW );
digitalWrite( MOTOR_A_PWM, LOW );
delay( DIR_DELAY );
// set the motor speed and direction
digitalWrite( MOTOR_A_DIR, HIGH ); // direction = forward
analogWrite( MOTOR_A_PWM, 255 - Speed ); // PWM speed = fast
//analogWrite( MOTOR_A_PWM, 255-PWM_FAST ); // PWM speed = fast
Serial.println(" OK");
}
if (buff[0] == 'L' && buff[1] == 'B') { // правый двигатель
int Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
// always stop motors briefly before abrupt changes
digitalWrite( MOTOR_A_DIR, LOW );
digitalWrite( MOTOR_A_PWM, LOW );
delay( DIR_DELAY );
// set the motor speed and direction
digitalWrite( MOTOR_A_DIR, LOW ); // direction = reverse
analogWrite( MOTOR_A_PWM, Speed ); // PWM speed = slow
Serial.println(" OK");
}
if (buff[0] == 'R' && buff[1] == 'F') { // правый двигатель
int Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
digitalWrite( MOTOR_B_DIR, LOW );
digitalWrite( MOTOR_B_PWM, LOW );
delay( DIR_DELAY );
// set the motor speed and direction
digitalWrite( MOTOR_B_DIR, HIGH ); // direction = forward
analogWrite( MOTOR_B_PWM, 255 - Speed ); // PWM speed = fast
Serial.println(" OK");
}
if (buff[0] == 'R' && buff[1] == 'B') { // правый двигатель
int Speed = (buff[3] - 48) * 100 + (buff[4] - 48) * 10 + (buff[5] - 48);
// always stop motors briefly before abrupt changes
digitalWrite( MOTOR_B_DIR, LOW );
digitalWrite( MOTOR_B_PWM, LOW );
delay( DIR_DELAY );
// set the motor speed and direction
digitalWrite( MOTOR_B_DIR, LOW ); // direction = reverse
analogWrite( MOTOR_B_PWM, Speed); // PWM speed = slow
Serial.println(" OK");
}
if (buff[0] == 'T' && buff[1] == 'E') {
// sensorValue = analogRead(tempPin); // read the sensor
// Serial.print(sensorValue);
SensorPrint();
Serial.println(" OK");
}
// зажигаем линейку
if (buff[0] == 'L' && buff[1] == 'E') {
int num = buff[3] - 48; // должен быть номен светодиода
if (buff[5] == '1') {
// bitWrite(bitsToSend, num , HIGH);
registerWrite(num, HIGH);
} else {
//bitWrite(bitsToSend, num , LOW);
registerWrite(num, LOW);
}
// registerWrite(bitsToSend);
}
ClearBuff(); // обнулить буфер, ждать сообщения
} else { // еще не конец сообщения, записать в буфер
if (input_size <= 5) {
buff[input_size] = incoming_byte;
input_size++;
continue;
}
input_size++; // вообще не должны сюда попадать, но если так случилось, то пусть добавит
Serial.print("ERR - ");
//Serial.print(input_size);
if (input_size>6) {
ClearBuff(); // обнулить буфер, ждать сообщения
}
}
}
AutoStop(); // если препятствие, то остановиться
}
void ShowFF() // шкалу засветить от
{
/*
for (int i=0; i<10;i++) {
bitWrite(bitsToSend, i, LOW);
}
registerWrite(0,LOW);
*/
if (cm < 50 ) bitWrite(bitsToSend, 0, HIGH);
if (cm >= 50 ) bitWrite(bitsToSend, 0, LOW);
if (cm < 45 ) bitWrite(bitsToSend, 1, HIGH);
if (cm >= 45 ) bitWrite(bitsToSend, 1, LOW);
if (cm < 40 ) bitWrite(bitsToSend, 2, HIGH);
if (cm >= 40 ) bitWrite(bitsToSend, 2, LOW);
if (cm < 35 ) bitWrite(bitsToSend, 3, HIGH);
if (cm >= 35 ) bitWrite(bitsToSend, 3, LOW);
if (cm < 30 ) bitWrite(bitsToSend, 4, HIGH);
if (cm >= 30 ) bitWrite(bitsToSend, 4, LOW);
if (cm < 25 ) bitWrite(bitsToSend, 5, HIGH);
if (cm >= 25 ) bitWrite(bitsToSend, 5, LOW);
if (cm < 20 ) bitWrite(bitsToSend, 6, HIGH);
if (cm >= 20 ) bitWrite(bitsToSend, 6, LOW);
if (cm < 15 ) bitWrite(bitsToSend, 7, HIGH);
if (cm >= 15 ) bitWrite(bitsToSend, 7, LOW);
if (cm < 10 ) bitWrite(bitsToSend, 8, HIGH);
if (cm >= 10 ) bitWrite(bitsToSend, 8, LOW);
if (cm < 5 ) bitWrite(bitsToSend, 9, HIGH);
if (cm >= 5 ) bitWrite(bitsToSend, 9, LOW);
regWrite();
}
// записывает два байта в регистры
void registerWrite( int whichPin, int whichState) {
// для хранения 16 битов используем unsigned int
//unsigned int bitsToSend = 0;
// выключаем светодиоды на время передачи битов
digitalWrite(latchPin, LOW);
// устанавливаем HIGH в соответствующий бит
bitWrite(bitsToSend, whichPin, whichState);
// разбиваем наши 16 бит на два байта
// для записи в первый и второй регистр
byte registerOne = highByte(bitsToSend);
byte registerTwo = lowByte(bitsToSend);
// "проталкиваем" байты в регистры
shiftOut(dataPin, clockPin, MSBFIRST, registerOne);
shiftOut(dataPin, clockPin, MSBFIRST, registerTwo);
// "защелкиваем" регистр, чтобы биты появились на выходах регистра
digitalWrite(latchPin, HIGH);
}
void regWrite() { // записывает глобальную переменную bitstosend в
// для хранения 16 битов используем unsigned int
//unsigned int bitsToSend = 0;
// выключаем светодиоды на время передачи битов
digitalWrite(latchPin, LOW);
// устанавливаем HIGH в соответствующий бит
// разбиваем наши 16 бит на два байта
// для записи в первый и второй регистр
byte registerOne = highByte(bitsToSend);
byte registerTwo = lowByte(bitsToSend);
// "проталкиваем" байты в регистры
shiftOut(dataPin, clockPin, MSBFIRST, registerOne);
shiftOut(dataPin, clockPin, MSBFIRST, registerTwo);
// "защелкиваем" регистр, чтобы биты появились на выходах регистра
digitalWrite(latchPin, HIGH);
}
void i2cWrite(uint8_t registerAddress, uint8_t data) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
uint8_t data[nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for (uint8_t i = 0; i < nbytes; i++)
data [i] = Wire.read();
return data;
}
void SensorPrint()
{
Serial.print("Way:");
Serial.print(Way);
Serial.print(" ");
Serial.print("CurWay:");
Serial.print(CurWay);
Serial.print(" ");
Serial.print("MaxWay:");
Serial.print(MaxWay);
Serial.print(" ");
Serial.print("Z:");
Serial.print(GYRO_ANGLE_Z);
Serial.print(" ");
sensorValue = analogRead(tempPin); // read the sensor
// Kelvin = (((float(sensorValue) / 1023) * 5) * 100); // convert to Kelvin
// Celsius = Kelvin - 273.15-20; // convert to Celsius
Serial.print("T1:");
Serial.print(sensorValue / 340.00 + 36.53);
Serial.print(" ");
Serial.print("T2:");
Serial.print (tempRaw / 340.00 + 36.53);
Serial.print(" ");
sensorValue = analogRead(flPin); // read the sensor
Serial.print("FL:");
Serial.print(sensorValue);
Serial.print(" ");
sensorValue = analogRead(frPin); // read the sensor
Serial.print("FR:");
Serial.print(sensorValue);
Serial.print(" FF:");
Serial.print(cm);
Serial.println(" ");
}
void FuncCC02() // моргание зеlеным и красными светодиодами попеременке
{
unsigned long T0 = millis();
if (T0 < T1) { // это пауза все погасили если один горит
/*
registerWrite(10, LOW); // 10,13 - красный
registerWrite(11, LOW); // 10,13 - красный
registerWrite(12, LOW); // 11,14 - синий
registerWrite(13, LOW); // 10,13 - красный
registerWrite(14, LOW); // 10,13 - красный
registerWrite(15, LOW); // 12,15 - зеленый
*/
return;
}
if (T0 >= T1 && T0 <= T2) { //это горим bitRead(bitsToSend, 11)==1
if (leftLED) {
registerWrite(10, HIGH); // 10,13 - красный
registerWrite(15, HIGH); // 12,15 - зеленый
}
if (! leftLED) {
registerWrite(13, HIGH); // 10,13 - красный
registerWrite(12, HIGH); // 12,15 - зеленый
}
}
if (T0 > T2) { // время вышло, нужно переопределить счетчики
T1 = T0 + 1000;
T2 = T0 + 1100;
leftLED = !leftLED;
rightLED = !rightLED;
bitWrite(bitsToSend, 10, LOW);
bitWrite(bitsToSend, 11, LOW);
bitWrite(bitsToSend, 12, LOW);
bitWrite(bitsToSend, 13, LOW);
bitWrite(bitsToSend, 14, LOW);
bitWrite(bitsToSend, 15, LOW);
//registerWrite(10, LOW); // 10,13 - красный
//registerWrite(11, LOW); //
//registerWrite(12, LOW); // 11,14 - синий
//registerWrite(13, LOW); //
//registerWrite(14, LOW); //
// записываем все за один раз
registerWrite(15, LOW); // 12,15 - зеленый
return;
}
}
// оба назад
void Reverse(int Speed)
{
Stop(); // сначала остановить
// set the motor speed and direction
digitalWrite( MOTOR_A_DIR, LOW ); // direction = reverse
analogWrite( MOTOR_A_PWM, Speed ); // PWM speed = slow
// set the motor speed and direction
digitalWrite( MOTOR_B_DIR, LOW ); // direction = reverse
analogWrite( MOTOR_B_PWM, Speed); // PWM speed = slow
}
void AutoStop()
{
if (MaxWay < 1 && MaxWay>-100 && Movement != STOP) { // стоп машина, проехали весь путь заявленный
Movement = STOP; // пока препятствие - тормозим
LastMovement = STOP; // остановились полностью
Stop();
MaxWay=-100; // чтобы не смотреть еще раз, пока не установлено повторно можно кататься
}
// алгоритмы автоматической остановки
if (cm <=15 && Movement == LEFT ) { // стоп машина, если едем и мало места пере нами
Movement = STOP; // пока препятствие - тормозим
LastMovement = STOP; // остановились полностью
Stop();
}
if (cm <=15 && Movement == RIGHT ) { // стоп машина, если едем и мало места перед нами
// голова должна смотреть направо сразу
Movement = STOP; // пока препятствие - тормозим
LastMovement = STOP; // остановились полностью
Stop();
}
if (cm <=15 && Movement == FORWARD ) { // стоп машина, если едем и мало места пере нами
Movement = STOP; // пока препятствие - тормозим
LastMovement = FORWARD;
Stop();
}
/*
if (cm <10 && Movement == STOP) { // назад едем, если подносят руку, а мы стояли
Stop();
Reverse(Speed);
Movement = REVERSE;
// set the motor speed and direction
}
*/
if (cm >15 && Movement == STOP && LastMovement == FORWARD) { // опять поехали
Stop();
Forward(Speed);
Movement = FORWARD;
// set the motor speed and direction
}
/*
if (cm >10 && Movement==REVERSE && LastMovement == STOP) { // останавливаемся, если ехали назад от руки, а ее убрали
Stop();
Movement = STOP;
// set the motor speed and direction
}
*/
if (cm >10 && Movement==REVERSE && LastMovement == FORWARD) { // препятствий нет, поехали опять вперед
Stop();
Forward(Speed);
Movement = FORWARD;
// set the motor speed and direction
}
}
void Stop()
{
digitalWrite( MOTOR_A_DIR, LOW );
digitalWrite( MOTOR_A_PWM, LOW );
digitalWrite( MOTOR_B_DIR, LOW );
digitalWrite( MOTOR_B_PWM, LOW );
delay( DIR_DELAY );
}
void Forward(int Speed)
{
Stop(); // сначала остановить
// set the motor speed and direction
myservo.write(90);
TestDistance();
if (cm<15)
{
myservo.write(90);
return;
}
digitalWrite( MOTOR_A_DIR, HIGH ); // direction = forward
analogWrite( MOTOR_A_PWM, 255-Speed ); // PWM speed = slow
// set the motor speed and direction
digitalWrite( MOTOR_B_DIR, HIGH ); // direction = forward
analogWrite( MOTOR_B_PWM, 255-Speed); // PWM speed = slow
}
void Left(int Speed)
{
Stop(); // сначала остановить
// посмотрите налево
myservo.write(150);
TestDistance();
if (cm<15)
{
myservo.write(90);
return;
}
// set the motor speed and direction
digitalWrite( MOTOR_B_DIR, HIGH ); // direction = forward
analogWrite( MOTOR_B_PWM, 255-Speed ); // PWM speed = slow
// set the motor speed and direction
digitalWrite( MOTOR_A_DIR, LOW ); // direction = forward
analogWrite( MOTOR_A_PWM, Speed); // PWM speed = slow
}
void Right(int Speed)
{
Stop(); // сначала остановить
myservo.write(20); // посмотрите направо
TestDistance();
if (cm<15)
{
myservo.write(90);
return;
}
digitalWrite( MOTOR_B_DIR, LOW ); // direction = forward
analogWrite( MOTOR_B_PWM, Speed ); // PWM speed = slow
// set the motor speed and direction
digitalWrite( MOTOR_A_DIR, HIGH ); // direction = forward
analogWrite( MOTOR_A_PWM, 255-Speed); // PWM speed = slow
}
void TestDistance()
{
int duration;//, cm;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
cm = duration / 58; // здесь записывается в глобальную переменную
}
void ClearBuff() // обнуление буфера
{
int i = 0;
while (i <= 6) {
buff[i] = 0;
i++;
}
input_size = 0;
}
