Установка датчиков столкновения

Сегодня установил сенсоры столкновения. Размышлял как бы их сделать, в результате родилась такая конструкция.

От алюминиевой пластины отрезал 2 полосы, выгнул их полудугами и с одной стороны загнул вокруг болтов.
IMGP2629_s
IMGP2630_s

Микрики установил на разной высоте используя брусочки разной высоты.
IMGP2632_s

IMGP2631_s

Сами микрики подключил между “землей” и входами 8,9. Допилил код. Углы поворота и отъезд назад регулировал задержкой. Задержка в 1сек примерно соответствует повороту на 90 градусов.

В коде соответственно сделал при замыкании левого сенсора робот поворачивает вправо и при замыкании правого сенсора – влево. Добавил также событие прямого столкновения, когда нажимаются оба сенсора. Единственное уже после просмотра видео понял что при прямом столкновении нужно сделать разворот на 180 градусов, иначе робот может долго крутиться на одном месте.

Вот само видео

А вот текущий код

// Определяем какие пины за что отвечают
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; // включен ли свет?

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);
}
//------------------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) {
boolean leftSens = digitalRead(SENS_LEFT);
boolean righSens = digitalRead(SENS_RIGH);
forward(); // ВПЕРЁД
if (!leftSens && righSens) {
back (); // НАЗАД
delay(1000);
left(); // В ЛЕВО
delay(1000);
forward(); // ВПЕРЁД
}
if (leftSens && !righSens) {
back (); // НАЗАД
delay(1000);
right(); // В ПРАВО
delay(1000);
forward(); // ВПЕРЁД
}
if (!leftSens && !righSens) {
back (); // НАЗАД
delay(2000);
right(); // В ПРАВО
delay(2000);
forward(); // ВПЕРЁД
}
}
}

На очереди проба применения цифрового компаса для компенсации разности вращения двигателей и точного угла поворота.

Поделиться в соц. сетях

0
Запись опубликована в рубрике Мой робот. Добавьте в закладки постоянную ссылку.

Добавить комментарий