В первой части мы переделали бензиновую газонокосилку в самоходную электрическую с помощью деталей от б/у гироскутера: двух мотор-колес, основной платы и аккумулятора. И добавили возможность управлять газонокосилкой с телефона через Wi-Fi.
В этой части газонокосилка научится определять высоту скошенной травы и получит простой автопилот на базе ESP8266. С его помощью она сможет косить траву примерно так же, как делает человек - двигаясь по границе скошенного участка и автоматически подруливая, чтобы оставаться на линии.
Часть 1. Механика и радиоуправление
Часть 2. Определение высоты травы
Часть 3. Сегментация травы нейросетью
Часть 4. Карта газона на визуальных маркерах
Если научить газонокосилку определять разницу в высоте между скошенной и нескошенной травой, то она сможет двигаться по линии между ними. Это достаточно эффективный способ за минимальное время покрыть площадь, которую нужно прокосить. Потому что в нем минимум поворотов, основное движение делается по длинным прямым линиям. И именно так косят газон люди, когда делают это вручную.
У этого способа есть большое преимущество - не требуется подготовка карты местности. Также это очень простой способ, ему не нужна большая вычислительная мощность. Поэтому с ним справится тот же микроконтроллер ESP8266, который управляет моторами газонокосилки и отвечает за радиоуправление с телефона.
Но есть и недостатки. Это локальный метод, он не может возвращать газонокосилку на базу или целенаправленно двигаться к островкам из непрокошеных участков.
Также он не может отличать бетонные дорожки и плоские предметы на земле, если они не выступают за уровень поверхности. А выступающие он будет воспринимать как нескошенную траву. Если с препятствиями в виде кустов и деревьев еще можно справиться датчиками расстояния или контактным бампером, то этот недостаток определением только разницы в высоте принципиально непреодолим.
Но есть и хорошие новости. Если вручную или на радиоуправлении прокосить кольцо вокруг деревьев, а также полосы вдоль дорожек, то такой способ не будет на них заезжать (так как там уже прокошено). Получается хоть и с частично ручным трудом, но зато достаточно надежное объезжание препятствий без дополнительных датчиков.
Датчики расстояния
Существуют два основных популярных датчика расстояния, знакомых каждому ардуинщику. Это ультразвуковой HC-SR04 (цена 50 рублей) и относительно недавно появившиеся лазерные дальномеры VL53L0X (цена 120 рублей).
Они оба обладают сравнимыми характеристиками: измеряемое расстояние около 2 метров, частота измерений 10-20 раз в секунду. Даже угол зрения у них похож, примерно 30 градусов. Другими словами, они оба измеряют не расстояние до точки впереди, а до всего что попадет в конус с углом 30 градусов.
Лазерный VL53L0X работает по протоколу I2C и намного меньше по размеру. Смотрите масштаб на фото выше. К сожалению, не смотря на использование шины I2C, нельзя несколько VL53L0X повесить на общие четыре провода (два для шины I2C и два для питания). Потому что они с завода имеют одинаковый адрес. А для смены адреса к каждому датчику VL53L0X нужно тянуть отдельный провод.
А ультразвуковой HC-SR04 для точной работы требует поддержки микроконтроллером прерываний. Так как его принцип работы состоит в том, что надо подать сигнал на пин TRIGGER, после чего датчик испускает серию ультразвуковых волн, а после получения отраженного сигнала, он изменяет состояние своего пина ECHO. Поэтому приходится на ECHO вешать прерывание, чтобы измерять точные временные интервалы.
Из недостатков этих датчиков нужно отметить, что на VL53L0X может влиять яркий солнечный свет, а у HC-SR04 трава довольно сильно глушит звуки. Но на практике они оба прекрасно работают днем. И даже точность при работе по траве у них примерно одинаковая.
Ниже мы добавим в автопилот возможность работы с обоими этими датчиками, выбирайте какой вам удобней или какой лучше покажет себя в ваших условиях.
Алгоритм работы
В минимальной конфигурации для такого автопилота нужен только один датчик HC-SR04 или VL53L0X.
Если разместить дальномер в левой передней части газонокосилки на границе между травой и скошенной поверхностью (обычно это 5-10 см от левого края кожуха косилки в сторону корпуса), то этого будет вполне достаточно для работы этого метода.
Если в этом месте расстояние от датчика до травы будет меньше некоторого порога, то это означает, что в этом месте трава высокая, а значит нужно поворачивать влево. В сторону скошенной полосы. А если датчик покажет расстояние больше порога, то значит там трава низкая, то есть уже скошенная. Это расстояние до голой земли. И значит надо повернуть вправо, в сторону еще не скошенной высокой травы.
Так газонокосилка будет двигаться по краю нескошенной травы, постоянно подруливая влево-вправо, но в среднем оставаясь на нужной линии.
Эта задача известна как "следование по линии" и является базовой на кружках робототехники. Машинка едет по нарисованной черной линии на полу, стараясь не съезжать с нее.
Обычно там используется ряд из 2-8 фотодиодов, которые позволяют определить текущий угол отклонения от линии. Этот угол используется в PID контроллере, чтобы при больших углах поворачивать резче, а при маленьких более плавно. Интегральный и дифференциальный компонент в формулах PID помогают ехать ровно, без колебаний относительно линии.
К счастью, так как мы не просто фиксируем черное пятно под датчиком, а измеряем именно расстояние, которое показывает высоту травы. Которая однозначно определяет в какую сторону нам нужно повернуть. Поэтому в минимальной конфигурации нам достаточно только одного датчика. Хотя можно использовать и несколько, конечно.
В качестве разного по величине угла отклонения от линии при одном датчике, можно использовать время. Если мы долго движемся по плоской поверхности, то надо сильнее повернуть направо в сторону травы. И наоборот, если мы выехали на высокую траву и долго не можем вернуться на уже прокошенную полосу, то нужно увеличить степень поворота налево.
Собственно, алгоритм и формулы можно брать напрямую из тех задач с машинками. Но стоит отдельно упомянуть один трюк, который используется в таких машинках, чтобы лучше следовать линии. Там есть два режима: один езды на базовой скорости, подруливая влево-вправо согласно углу отклонения от линии. Но если этот угол становится слишком большим, то машинка останавливается и начинает вращаться на месте.
Мы будем использовать именно такой продвинутый вариант.
Программа
Как и в прошлый раз, программировать ESP8266 будем на языке javascript с помощью Espruino. Установка и настройка описаны в предыдущей части.
Воспользуемся по максимуму предыдущей программой, сохранив возможность управлять газонокосилкой с телефона по Wi-Fi. Просто добавим еще одну кнопку в интерфейс с названием Auto, при нажатии на которую будет включаться автопилот.
Любое движение джойстиком будет сбрасывать режим автопилота и переключать на ручное управление. Так будет обеспечена безопасность, так как можно будет алгоритм прервать в любой момент.
Технически, мы все еще будем слать на газонокосилку команды каждые 20-40 миллисекунд, чтобы отслеживать со стороны ESP8266 возможный обрыв связи. Это тоже увеличит безопасность, при пропадании связи дольше 300 миллисекунд газонокосилка автоматически остановится.
Но в режиме автопилота вместо команд /M, мы будем слать команды /MA, обозначающие автоматический режим.
Чтобы не мучаться с обработкой и хранением настроек на самой ESP8266, мы будем каждый раз в строке /MA слать полный набор параметров, нужных для автопилота. При скоростях Wi-Fi несколько лишних символов в строке запроса на скорости никак не сказываются, а это позволит нам динамически менять параметры автопилота из HTML интерфейса. И так отлаживать настройки для конкретной газонокосилки.
И параметры эти следующие:
/MAdist,speed,rotate,rotate2,rotate_timeout,turn,turn_timeoutЗдесь dist - это расстояние в сантиметрах от датчика до травы. Расстояния меньше этого значения будут означать, что мы находится над высокой травой. А расстояния меньше - что над плоской землей, т.е. над уже прокошенным участком.
speed - базовая скорость движения вперед в обычном режиме подруливания (аналог как у машинок, следующих по линии). В пределах -1000...1000.
rotate и rotate2, - это степень поворота при подруливании, тоже -1000...1000. Причем степень подруливания линейно увеличивается от rotate до rotate2 в течение времени rotate_timeout в миллисекундах. Это позволит нам увеличивать угол поворота, если мы долго находится над высокой травой (или над скошенной). Аналог угла отклонения от линии у машинок.
turn - остановка и поворот с такой скоростью на месте, если за предыдущее время rotate_timeout так и не смогли выйти на скошенную (или на высокую) траву. Это аналог второго режима у машинок для слишком большого угла отклонения.
Если же за время turn_timeout в миллисекундах мы так и не сможем найти границу травы, то полностью останавливаемся и ждем команды оператора. Потому что дальше неизвестно что делать, мы потеряли слежение. Газонокосилка при этом начнет издавать по три коротких звука для сигнализации, что она застряла в этом жестоком непредсказуемом мире.
Итоговая строка отправляемых на газонокосилку команд для автоматического режима может выглядеть так:
/MA25,100,80,100,3000,200,5000Здесь порог расстояния до травы 25 см, двигаться вперед с базовой скоростью 100 (из диапазона 1000, то есть на 10% газа), подруливать начиная от уровня 80, плавно увеличивая до 100 в течении 3000 миллисекунд (3 секунд). Если же за 3 секунды косилка не сможет найти границу травы, то остановиться и поворачивать с одинаковой скоростью 200 в течение 5000 миллисекунд (5 секунд). Если же и после этого не удастся найти, то полностью остановиться.
Такие параметры нормально работали у меня. Но у вас могут более оптимальными быть другие. Для этого в HTML интерфейс в раздел настроек добавим поле ввода, где их можно оперативно изменять с телефона и смотреть какие подходят лучше
auto <input id='i_auto' type=text value='25,100,80,100,3000,200,5000'>Также добавим отдельную кнопку для включения и выключения датчика расстояния. По умолчанию он будет выключен и включаться только при авто режиме. Эта кнопка просто шлет специальную команду /D в ESP8266
<button onclick="send('D')">dist</button>Также добавим новую кнопку с названием Auto, которая переключает режим автопилота. Для наглядности она будет подсвечиваться красным цветом при включенном автопилоте. Помните, что любое движение джойстиком сбрасывает автопилот. Это очень удобно.
Если заметили, в левом верхнем углу теперь показывается текущее расстояние с датчика-дальномера в сантиметрах.
И текущий уровень газа на моторы, который выдает ESP8266. В виде speed,steer (каждое значение в диапазоне -1000....1000).
Это нужно для отслеживания работы автопилота, в качестве обратной связи от ESP8266. Так как в автоматическом режиме теперь ESP8266 сама решает, какой уровень газа дать на каждый из моторов (в зависимости от показаний датчика травы), то для нас будет очень удобно смотреть за этим на экране телефона.
В данном случае газонокосилка едет вперед со скоростью 100 (10% полного газа) и поворачивает вправо с уровнем поворота 93 (9.3% от максимума). Факт наличия поступательной скорости 100 единиц говорит о том, что автопилот находится в первом режиме: движение вперед с постоянным подруливанием. Если бы он перешел на второй режим с поворотами на месте, то там первое значение стояло бы 0, а второе 200 (как задано в настройках отправляемой строки /MA): 26 cm 0,200
Такая обратная связь в коде реализуется очень просто: в конце каждого GET запроса вместо ОК мы теперь возвращаем готовую строку:
res.end(A.status+" "+speed+","+steer);Здесь в A.status сохранено текущее расстояние "26 cm".
А в HTML интерфейсе на телефоне, если ответ сервера в функции fetch() отличается от "ОК", то выводим на экран все что он прислал:
if (data!='OK') info.innerText = data;Настройки автопилота в коде для удобства хранятся в одном объекте А
A = {
sensor_type: 2, // 0 - без дальномера, 1 - лазерный VL53L0X, 2 - ультразвуковой HC-SR04
dist_grass: 30, // cm
speed: 300, // move base, 0..1000
rotate: 400,
rotate2: 600,
rotate_timeout: 3000, // ms
turn: 300, // rotate in place, 0..1000
turn_timeout: 5000,// ms
is_auto: false,
is_dist: false,
sensor: null, // HC-SR04 или VL53L0X
dist: 0.0, // текущее расстояние, cm
time_rotate: 0,
time_turn: 0,
mode: 0, // -2,-1,0,1,2, 5 (-2 - turn, -1 - rotate, 0 - stop, 5 - begin auto)
status: "",
}Здесь обратите внимание на A.sensor_type, с помощью которого можно выбрать используемый датчик расстояния: ультразвуковой HC-SR04 или лазерный VL53L0X (сейчас выбран HC-SR04).
Можно также отключить оба датчика при sensor_type: 0, тогда телефон будет просто пультом радиоуправления газонокосилкой, как было в первой части.
Дальше идут текущие настройки автопилота, такие же как в строке /MA, а ниже еще несколько служебных переменных, необходимых для работы автопилота.
При каждом получении команды /MA со списком параметром, мы ее парсим и сохраняем настройки автопилота в объект A:
} else if (msg[1]=="M" && msg[2]=="A") {
// /MAdist,speed,rotate,rotate2,rotate_timeout,turn,turn_timeout
var arr = msg.split(",");
A.dist_grass = arr[0].substring(3)*1;
A.speed = arr[1]*1;
A.rotate = arr[2]*1;
A.rotate2 = arr[3]*1;
A.rotate_timeout = arr[4]*1;
A.turn = arr[5]*1;
A.turn_timeout = arr[6]*1;
if (!A.is_auto) A.mode = 5; // begin auto
A.is_auto = true;
A.is_dist = true;
}Получение команды /MA включает и датчик (A.is_dist = true), и режим автопилота (A.is_auto = true). А каждое получение команды /M отключает автопилот:
} else if (msg[1]=="M") {
// /M-1000,1000
A.is_auto = false;
....Таймаут при обрыве связи в функции check_timeout() тоже его отключает, чтобы газонокосилка остановилась.
И последнее отличие от предыдущей версии программы для ESP8266, это собственно функция автопилота auto(). Она запускается по таймеру в onInit() каждые 100 мс, то есть по 10 раз в секунду.
Функция автопилота auto()
function auto(){
if (A.sensor && (A.is_dist || A.is_auto)) {
A.status = A.dist.toFixed(0)+" cm";
if (A.sensor_type == 1) {
try{
A.dist = A.sensor.performSingleMeasurement().distance / 10; // VL53L0X, mm -> cm
}catch(e){A.status='error';}
} else {
A.sensor.trigger(); // HC-SR04, send pulse, result in A.dist
}
}
if (!A.is_auto || A.dist < 5 || A.dist > 100 || A.mode == 0) return; // 5..100 cm
var time = Date.now(); // ms
if (A.dist <= A.dist_grass) {
// grass, rotate left
if (A.mode == -2) {
// turn left
if (time - A.time_turn > A.turn_timeout) {
// stop
speed = 0;
steer = 0;
A.mode = 0; // stop
} else {
// turn
speed = 0; // in place
steer = -A.turn; // left
}
} else {
// rotate left
if (A.mode != -1) {A.mode = -1; A.time_rotate = time;} // begin rotate
if (time - A.time_rotate > A.rotate_timeout) {
// timeout rotate
A.mode = -2;
A.time_turn = time; // begin turn
} else {
// PID
speed = A.speed; // base move
steer = -(A.rotate + Math.floor((A.rotate2-A.rotate)*(time - A.time_rotate)/A.rotate_timeout)); // rotate...rotate2, left
}
}
//---
} else {
// no grass, rotate right
if (A.mode == 2) {
// turn right
if (time - A.time_turn > A.turn_timeout) {
// stop
speed = 0;
steer = 0;
A.mode = 0; // stop
} else {
// turn
speed = 0; // in place
steer = A.turn; // right
}
} else {
// rotate right
if (A.mode != 1) {A.mode = 1; A.time_rotate = time;} // begin rotate
if (time - A.time_rotate > A.rotate_timeout) {
// timeout rotate
A.mode = 2;
A.time_turn = time; // begin turn
} else {
// PID
speed = A.speed; // base move
steer = A.rotate + Math.floor((A.rotate2-A.rotate)*(time - A.time_rotate)/A.rotate_timeout); // rotate...rotate2, right
}
}
}
send_motors();
}
Она запутанная по структуре, но очень простая по логике.
Сначала получаем расстояние до травы от датчика (это делается с частотой вызова функции auto(), то есть 10 раз в секунду). Лазерный VL53L0X сразу возвращает значение, только его нужно разделить на 10, чтобы перевести миллиметры в сантиметры. Но VL53L0X может выдавать ошибки в I2C шине, поэтому эту функцию нужно заключить в блок try/catch
A.dist = A.sensor.performSingleMeasurement().distance / 10;А у ультразвукового HC-SR04 нужно только вызвать начало измерения. Результат запишется в переменную A.dist в обратном вызове.
A.sensor.trigger();Беспокоиться о том, что переменная с расстоянием может быть повреждена при записи в нее из прерывания не нужно, Espruino каждое прерывание просто запоминает в очереди, вместе с его точным временем наступления. А обработку самого тела функции делает в общем цикле в однопоточном режиме.
После, в зависимости от расстояния до травы A.dist, принимаем решение в какую сторону поворачивать. При этом запоминаем время, чтобы отслеживать таймауты. При превышении первого начинаем вращение на месте, а при превышении второго вообще останавливаемся насовсем.
Выше я упоминал, что в случае полной остановки газонокосилка будет издавать по три звуковых сигнала. Это делается тем, что просто перестаем слать команды по UART, так как из-за A.mode = 0 до последней строчки с send_motors(); скрипт в этом случае не дойдет. Немного грязный хак, но полезный. Чем все время смотреть в экран телефона, чтобы отследить момент когда автопилот сдался.
Инициализация самих датчиков делается в onInit().
VL53L0X
VL53L0X инициализируется довольно просто:
try{
I2C1.setup({ scl : pin_TRIG, sda: pin_ECHO , bitrate: 100000}); // bitrate: 15000 - 400000, по умолчанию 100000 (100кГц)
A.sensor = require("VL53L0X").connect(I2C1);
}catch(e){A.status='error init VL53L0X';}Модуль VL53L0X.js для работы с этим датчиком Espruino IDE скачивает автоматически (но можно скачать вручную и положить в папку modules, если в настройках IDE указана папка с проектом).
Этот код надо заключать в блок try/catch, так как он может выдавать ошибку, если не удалось инициализировать VL53L0X. Но благодаря A.status, об этом будет сообщено на экране телефона.
Подключается VL53L0X как пин SDA к пину D5 на ESP8266, а пин SCL к D6.
Питание у VL53L0X +3.3V, поэтому подсоединяется напрямую к ESP8266.
ВНИМАНИЕ! У меня VL53L0X по неизвестным причинам иногда начинает выдавать сплошные ошибки в шине I2C (No ASK). Причем сброс микроконтроллера не помогает, только полное отсоединение VL53L0X от питания. Возможно, это что-то с моими модулями или с прошивкой Espruino, а возможно на длинных провода I2C сама по себе работает нестабильно. Можно попробовать подтянуть SDA и SCL к +3.3V внешними подтягивающими резисторами. В шине I2C на обоих этих проводах здесь всегда должно быть напряжение +3.3V, а устройства при передаче данных замыкают их на 0.
Как подключить несколько датчиков VL53L0X?
Есть два способа. Либо использовать внешнюю плату типа TCA9548, которая позволяет переключаться между несколькими I2C датчиками с одинаковым адресом. Либо к каждому VL53L0X подвести по дополнительному проводу к пину XSHUT. Замыкая его на землю с помощью (OUTPUT, LOW), можно на время отключить датчик, а переведя его в (INPUT), включить обратно.
Тогда алгоритм такой: с помощью пина XSHUT выключаем все VL53L0X, потом включаем по одному и с помощью I2C команды меняем его адрес на какой-то другой. После этого у всех датчиков будут разные адреса и они прекрасно будут работать на одной I2C шине.
Ниже под спойлером пример кода для инициализации двух VL53L0X внутри функции onInit(). С большим числом датчиков это делается аналогично.
function onInit() {
// эта функция запускается при старте
// выключаем оба (OUTPUT, LOW)
digitalWrite(pin_X1, 0);
digitalWrite(pin_X2, 0);
// set up I2C
I2C1.setup({ scl : pin_SCL, sda: pin_SDA , bitrate: 100000}); // bitrate: 15000 - 400000, по умолчанию 100000 (100кГц)
// включаем первый
pinMode(pin_X1, 'input');
laser1 = require("VL53L0X").connect(I2C1, {address:0x54 });
// включаем второй
pinMode(pin_X2, 'input');
laser2 = require("VL53L0X").connect(I2C1, {address:0x56 });
// создаем HTTP сервер
server = require("http").createServer(onPageRequest).listen(PORT);
}HC-SR04
А с HC-SR04 еще проще. Его инициализация сразу включает callback функцию, куда передается расстояние в сантиметрах, когда оно появляется в датчике. Поэтому мы его просто копируем в переменную A.dist
A.sensor = HC_SR04(pin_TRIG,pin_ECHO,function(d) {
A.dist = d; // cm
});Для начала нового измерения, по-прежнему надо вызвать A.sensor.trigger(), что делается в функции auto() по 10 раз в секунду.
ВНИМАНИЕ! Модуль HC-SR04.js в Espruino для работы с датчиком HC-SR04... Не работает с ним! Точнее, ответ от HC-SR04 приходит через полторы секунды. Проблема во времени с триггером.
Поэтому я просто вставил код исправленного модуля (он небольшой) в саму программу. Вот так это выглядит:
Исходный код
// HC-SR04
HC_SR04 = function(/*=PIN*/trig, /*=PIN*/echo, callback) {
var riseTime = 0;
trig.reset(); // lower the trigger, as it would have been floating
setWatch(function(e) { // check for rising edge
riseTime=e.time;
}, echo, { repeat:true, edge:'rising' });
setWatch(function(e) { // check for falling edge
callback(((e.time-riseTime)*1000000)/57.0);
}, echo, { repeat:true, edge:'falling' });
return {
trigger : function() {
digitalPulse(trig, 1, 0.1); // FIX: было 0.01 /*10uS*/, из-за чего время ответа было 1600 мс
}
};
};Вся остальная работа с HC-SR04 остается без изменений.
ВНИМАНИЕ! Большинство датчиков HC-SR04 с алиэкспресс работают только с напряжением +5V. Которое нужно подавать на их пин TRIGGER для начала измерения. Хотя иногда встречаются модели, способные работать в том числе и на +3.3V, но это большая редкость.
Поэтому при подключении HC-SR04 к ESP8266 придется делать это через конвертор уровней из +3.3V в +5V. К счастью, на NodeMCU V3 есть пин VU, на котором есть +5V. Его можно использовать для запитывания конвертора уровней. Но +5V на пине VU есть только при питании самой ESP8266 от USB, то есть от внешнего повербанка. Как всегда, работа с микроконтроллерами сплошная попаболь.
Подключите пин ECHO к D5 на ESP8266, а пин TRIG к D6. И +5V к VIN у HC-SR04. Ну и GND, конечно.
Итоговый скрипт для ESP8266
Под спойлером ниже полный финальный код для ESP8266.
Финальный код для ESP8266
pin_ECHO = NodeMCU.D5; // ECHO у HC-SR04 или SDA у VL53L0X
pin_TRIG = NodeMCU.D6; // TRIG у HC-SR04 или SCL у VL53L0X
wheel_4 = false; // 2 платы?
pin_4 = NodeMCU.D2; // пин для UART второй платы
TIMEOUT = 300; // таймаут связи, мс
speed = 0; // газ
steer = 0; // повороты
var server;
serial2 = null;
time_last = 0; // время последней команды
// для бинарных данных по UART
buffer = new ArrayBuffer(8); // 8 байт в одной UART команде
view = new DataView(buffer); // нужно для записи uint16_t и int16_t в buffer
// режим auto
A = {
sensor_type: 1, // 0 - без дальномера, 1 - лазерный VL53L0X, 2 - ультразвуковой HC-SR04
dist_grass: 30, // cm
speed: 300, // move base, 0..1000
rotate: 400,
rotate2: 600,
rotate_timeout: 3000, // ms
turn: 300, // rotate in place, 0..1000
turn_timeout: 5000,// ms
is_auto: false,
is_dist: false,
sensor: null, // HC-SR04 или VL53L0X
dist: 0.0, // текущее расстояние, cm
time_rotate: 0,
time_turn: 0,
mode: 0, // -2,-1,0,1,2, 5 (-2 - turn, -1 - rotate, 0 - stop, 5 - begin auto)
status: "",
}
// HC-SR04
HC_SR04 = function(/*=PIN*/trig, /*=PIN*/echo, callback) {
var riseTime = 0;
trig.reset(); // lower the trigger, as it would have been floating
setWatch(function(e) { // check for rising edge
riseTime=e.time;
}, echo, { repeat:true, edge:'rising' });
setWatch(function(e) { // check for falling edge
callback(((e.time-riseTime)*1000000)/57.0);
}, echo, { repeat:true, edge:'falling' });
return {
trigger : function() {
digitalPulse(trig, 1, 0.1); // FIX: было 0.01 /*10uS*/, из-за чего время ответа было 1600 мс
}
};
};
function onInit() {
// эта функция запускается при старте
// UART на D4 (аппаратный Tx)
Serial2.setup(9600, {tx:NodeMCU.D4});
// софтверный UART для второй платы
if (wheel_4) {
serial2 = new Serial();
serial2.setup(9600,{tx:pin_4});
}
send_motors();
// timeout wifi
setInterval(check_timeout,TIMEOUT);
// создаем HTTP сервер
server = require("http").createServer(onPageRequest).listen(80);
if (A.sensor_type == 1) {
// VL53L0X over I2C
try{
I2C1.setup({ scl : pin_TRIG, sda: pin_ECHO , bitrate: 100000}); // bitrate: 15000 - 400000, по умолчанию 100000 (100кГц)
A.sensor = require("VL53L0X").connect(I2C1);
}catch(e){A.status='error init VL53L0X';}
} else if (A.sensor_type == 2) {
// HC-SR04
A.sensor = HC_SR04(pin_TRIG,pin_ECHO,function(d) {
A.dist = d; // cm
});
}
// auto mode (для HC-SR04 минимум 60 мс между измерениями, для VL53L0X минимум 20 мс)
if (A.sensor) setInterval(auto,100);
}
function check_timeout(){
if (Date.now() - time_last > TIMEOUT) {
// стоп
A.is_auto = false;
speed = 0;
steer = 0;
send_motors();
}
}
function onPageRequest(req, res) {
var msg = req.url;
res.writeHead(200, {'Content-Type': 'text/html', 'Access-Control-Allow-Origin': '*'});
// время прихода последней команды
time_last = Date.now();
// парсим строку
if (msg =="/") {
res.end(WEB);
return;
} else if (msg[1]=="D") {
// /D
A.is_dist = !A.is_dist;
if (!A.is_dist) A.dist = 0;
} else if (msg[1]=="M" && msg[2]=="A") {
// /MAdist,speed,rotate,rotate2,rotate_timeout,turn,turn_timeout
var arr = msg.split(",");
A.dist_grass = arr[0].substring(3)*1;
A.speed = arr[1]*1;
A.rotate = arr[2]*1;
A.rotate2 = arr[3]*1;
A.rotate_timeout = arr[4]*1;
A.turn = arr[5]*1;
A.turn_timeout = arr[6]*1;
if (!A.is_auto) A.mode = 5; // begin auto
A.is_auto = true;
A.is_dist = true;
} else if (msg[1]=="M") {
// /M-1000,1000
A.is_auto = false;
var ind = msg.indexOf(',',2);
var sp = parseInt(msg.substring(2,ind));
var st = parseInt(msg.substring(ind+1, msg.length));
// газ моторов (шлем в UART всегда)
if (sp >= -1000 && sp <= 1000 && st >= -1000 && st <= 1000) {
speed = sp;
steer = st;
send_motors();
}
}
res.end(A.status+" "+speed+","+steer);
}
function send_motors() {
// отправка текущего газа на моторы
view.setUint16(0, 0xABCD, true); // uint16_t start = 0xABCD, стартовые 2 байта сообщения, true - все little endian
view.setInt16(2, steer, true); // int16_t steer=-1000..1000
view.setInt16(4, speed, true); // int16_t speed=-1000..1000
view.setUint16(6, 0xABCD ^ steer ^ speed, true); // uint16_t checksum = start ^ steer ^ speed
Serial2.write(buffer); // write, чтобы отправить бинарные данные
if (wheel_4) serial2.write(buffer); // вторая плата
}
function auto(){
if (A.sensor && (A.is_dist || A.is_auto)) {
A.status = A.dist.toFixed(0)+" cm";
if (A.sensor_type == 1) {
try{
A.dist = A.sensor.performSingleMeasurement().distance / 10; // VL53L0X, mm -> cm
}catch(e){A.status='error';}
} else {
A.sensor.trigger(); // HC-SR04, send pulse, result in A.dist
}
}
if (!A.is_auto || A.dist < 5 || A.dist > 100 || A.mode == 0) return; // 5..100 cm
var time = Date.now(); // ms
if (A.dist <= A.dist_grass) {
// grass, rotate left
if (A.mode == -2) {
// turn left
if (time - A.time_turn > A.turn_timeout) {
// stop
speed = 0;
steer = 0;
A.mode = 0; // stop
} else {
// turn
speed = 0; // in place
steer = -A.turn; // left
}
} else {
// rotate left
if (A.mode != -1) {A.mode = -1; A.time_rotate = time;} // begin rotate
if (time - A.time_rotate > A.rotate_timeout) {
// timeout rotate
A.mode = -2;
A.time_turn = time; // begin turn
} else {
// PID
speed = A.speed; // base move
steer = -(A.rotate + Math.floor((A.rotate2-A.rotate)*(time - A.time_rotate)/A.rotate_timeout)); // rotate...rotate2, left
}
}
//---
} else {
// no grass, rotate right
if (A.mode == 2) {
// turn right
if (time - A.time_turn > A.turn_timeout) {
// stop
speed = 0;
steer = 0;
A.mode = 0; // stop
} else {
// turn
speed = 0; // in place
steer = A.turn; // right
}
} else {
// rotate right
if (A.mode != 1) {A.mode = 1; A.time_rotate = time;} // begin rotate
if (time - A.time_rotate > A.rotate_timeout) {
// timeout rotate
A.mode = 2;
A.time_turn = time; // begin turn
} else {
// PID
speed = A.speed; // base move
steer = A.rotate + Math.floor((A.rotate2-A.rotate)*(time - A.time_rotate)/A.rotate_timeout); // rotate...rotate2, right
}
}
}
send_motors();
}
//WEB = `Server start...`;
WEB = `
<html>
<meta charset="UTF-8" />
<meta name="viewport" content="width=200, initial-scale=1">
<style>
.slider {-webkit-appearance: none; background: #82E0AA; outline: none; position: absolute; height:30%;}
</style>
<span id="info" style="color:red;"> ⬤</span>
<button onclick="div_opt.style.display=='none'?div_opt.style.display='block':div_opt.style.display='none';">☰</button>
<div id='div_opt' style="display:none">
<input id='speed_m' type="range" min="0" max="1000" value ="500"> <input id='steer_m' type="range" min="0" max="1000" value ="400">
<br>move <input id='i_move' type=text size=1 value=220> <button onclick="send('D')">dist</button>
<br>auto <input id='i_auto' type=text value='25,100,80,100,3000,200,5000'>
</div>
<div style="height:calc(100vmin - 32px);touch-action:none; position:relative;">
<input type="range" id="speed_s" min="-1000" max="1000" value ="0" class="slider" style="transform: translateX(-32%) rotate(-90deg); top:32%; width:min(90%, 80vmin);" oninput="update(this)" onmouseup="reset(this)" ontouchend="reset(this)">
<input type="range" id="steer_s" min="-1000" max="1000" value ="0" class="slider" style="top:25%; left:40%; width: 55%;" oninput="update(this)" onmouseup="reset(this)" ontouchend="reset(this)">
<button style="top:75%;left:min(35%, 35vmin);height:15%;width:20%; position:absolute; user-select:none;" onmousedown="move(true)" onmouseup="move(false)" ontouchstart="move(true)" ontouchend="move(false)" ontouchcancel="move(false)">Move</button>
<button id='b_auto' style="top:75%;left:75%;height:15%;width:20%; position:absolute;" onclick="auto(!is_auto)">Auto</button>
</div>
<script>
send_interval = 15; // ms
speed = 0;
steer = 0;
is_move = false;
is_auto = false;
setTimeout(send_motors,1000);
async function send(msg) {
const controller = new AbortController();
const signal = controller.signal;
setTimeout(() => controller.abort(), 2000);
try {
const response = await fetch("http://192.168.4.1/"+msg, {signal});
if (!response.ok) throw new Error('error');
const data = await response.text();
if (data!='OK') info.innerText = data;
if (info.style.color != "green") info.style.color = "green";
} catch (error) {
info.style.color = "red";
}
if (msg[0] == 'M') setTimeout(send_motors, send_interval);
}
function send_motors() {
if (is_move) send('M'+i_move.value+","+steer);
else if (is_auto) send('MA'+i_auto.value);
else send('M'+speed+","+steer);
}
function reset(el) {
el.value = 0;
update();
}
function update(){
auto(false);
speed = Math.round((speed_s.value*1 +1000)*(speed_m.value*2)/2000 - speed_m.value);
steer = Math.round((steer_s.value*1 +1000)*(steer_m.value*2)/2000 - steer_m.value);
}
function move(v){
auto(false);
is_move=v;
}
function auto(v){
if (v) is_move = false;
is_auto = v;
if (is_auto) b_auto.style.background='red'; else b_auto.style.background='';
}
</script>
`;Укажите в нем в переменной sensor_type тип используемого датчика расстояния (1 - лазерный VL53L0X, 2 - ультразвуковой HC-SR04, 0 - никакой). Залейте в ESP8266 через Espruino IDE и введите в левой консоли
save()Чтобы сохранить программу в памяти ESP8266.
После подключения к газонокосилке (пин D4 подключается к ближнему к черному проводу в шлейфе, как и в прошлый раз, а GND к черному проводу) и подачи питания, появится новая Wi-Fi сеть с названием "ESP8266".
Подключитесь к ней и откройте в браузере адрес http://192.168.4.1
С веб сервера на ESP8266 загрузится HTML интерфейс пульта управления.
Результат
Для удобства отладки, я использовал только шасси от газонокосилки.
Ниже я собрал в видеоролике множество попыток следовать по границе скошенной травы с разными настройками автопилота.
Как видите, в принципе это работает. Но не очень надежно. Газонокосилка часто теряет слежение и уезжает вдаль в траву. Хотя иногда потом восстанавливается сама. Я позднее пытался (но недолго) проверить на более высокой траве. Было получше, но полностью проблему не решило.
В этих условиях с не очень высокой травой нужно было детектировать разницу в высоте около 4 см. Как показали наблюдения, показания датчиков как раз прыгают в этом диапазоне. Думаю, это из-за того что сигнал иногда попадает между травинками и поэтому длина пролета луча получается разной, вплоть до земли. Разницы между VL53L0X и HC-SR04 по точности я не заметил. Разве что VL53L0X время от времени зависал и требовал переподключения питания.
Можно попробовать сглаживать показания. Или поставить несколько датчиков расстояния в ряд, это тоже должно помочь.
Тем не менее, метод рабочий. Если довести его до ума, он может быть помощником при покосе через радиоуправление. Либо как вспомогательный метод при других алгоритмах автопилота. Хотя назвать его единственным и достаточным нельзя. На данном этапе (один день не слишком долгих испытаний) его эффективность оказалась неприемлема.
В следующей части мы научим газонокосилку отличать скошенную траву от нескошенной с помощью нейросети.
По логике работы это будет почти то же самое, что с дальномерами, но нейросеть сможет распознавать препятствия на траве. Отличать бетонные дорожки, грядки с низкими цветами, чтобы не наезжать на них, и тому подобное. Чего не могли сделать датчики расстояния, определяющие только высоту травы.
Комментариев нет:
Отправить комментарий