Среда, 17 января 2018 10:58

Lake Erie Mamba: Реконфигурируемый робот-змея

Оцените материал
(3 голосов)

Эта подвижная рептилия состоит из общедоступных деталей и может передвигаться несколькими разными способами, включая ползанье, движение по прямой линии, а также передвижение боком или перекатами.

Для перекатов змея сворачивается в находящееся в вертикальном положении кольцо и катится подобно колесу. Робот состоит из 12 отдельных сегментов с сервоприводами, которые соединены друг с другом металлическими кронштейнами. Сервоприводы приводятся в действие чипом Arduino Mega, а запитаны аккумулятором с напряжением 7.4 вольта. Контролировать змею можно с помощью четырехкнопочного брелока радиоуправления, но она может передвигаться и полностью автономно. Собрать такого робота можно  с использованием сервоприводов и кронштейнов разных типов. Если вы решите собрать такого собственноручно, то вам определенно пригодится инструкция ниже.

 

Шаг 1: Список компонентов

FJU65GIIX6FR8Z4.MEDIUM

  • 1 чип Arduino Mega (и сенсорная панель к нему, но это не обязательно)
  • 1 четырехкнопочный брелок радиоуправления и связанный с ним приемник Momentary M4
  • 12 сервоприводов
  • 12 С-образных кронштейнов
  • 12 боковых кронштейнов
  • 4 длинных С-образных кронштейнов
  • 1 литий-ионный аккумулятор
  • 12 пар колесиков Lego
  • 1 сервопривод постоянного вращения
  • 1 инфра-красный датчик расстояния и крепление для него
  • 1 микро-сервопривод и крепление
  • Несколько сенсорных кабелей и разъемов
  • 1 большое колесико
  • 1 батарейный отсек 5AA с круглым разъемом питания
  • Разнообразные гайки, винты, зажимы для проводов и обрезки застежки-липучки

Шаг 2: Сборка

FW2L58WJ16OXADD.MEDIUM

Lake Erie Mamba можно собрать несколькими разными способами, в зависимости от необходимого способа передвижения. Сейчас мы рассмотрим способ сборки, в результате которого робот сможет ползать, как самая настоящая змея. Другие способы сборки будут приведены позже.

FBO05AOJ0X53UBI.MEDIUM

FCPIAEEJ16OXAD8.MEDIUM

FE5BM2JJ16OWKOY.MEDIUM

FQPLRFCJ140EBSU.MEDIUM

F95MDPHJ16OWKNR.MEDIUM

F1QFT9UJ16OWKPC.MEDIUM

Каждый из 12 сегментов состоит из сервопривода, С-образного кронштейна, бокового кронштейна, зажима для проводов и пары колесиков Lego. В колесной оси каждой пары колесиков необходимо просверлить два винтовых отверстия для последующего присоединения к С-образному кронштейну. Для этой же цели мне пришлось просверлить два отверстия в зажимах. Чтобы не запутаться, сверяйтесь с изображениями выше. После того как все 12 сегментов соединены в единое целое, к ним нужно добавить «голову» и «хвост», на которые позднее будут установлены чип и батарейки. Они делаются из одного бокового кронштейна и двух длинных С-образных, соединенных вместе по тому же принципу, что и на фото выше.

В хвостовую часть змеи помещаются Arduino и батарейный отсек, который питает чип. Сервоприводы же питаются от 7.4-вольтового аккумулятора, который устанавливается в «голову» робота. При этом все сервоприводы должны быть подключены к чипу проводами. Чтобы сенсорные провода не путались и не мешали роботу двигаться, их необходимо скреплять зажимами, но не натягивайте их слишком туго. Сервоприводы должны подключаться через разъемы Arduino от 2 до 13 включительно. Батарейный отсек 5AA подключается к чипу напрямую, а аккумулятор лучше подключить к разъему питания сенсорной панели. Надежно закрепить Arduino и батареи на месте помогут полоски липучки.

Радиоуправление осуществляется с помощью четырехкнопочного брелока и приемника. Это самый простой способ управлять любым роботом на базе Arduino. Контакт GND приемника сигнала подключается напрямую к контакту GND Arduino. Затем подключаем контакт питания к 5-вольтному контакту чипа (при этом не трогая 7.4-вольный контакт), а контакты от D0 до D3 подключаются к контактам Arduino от 14 до 17 соответственно (см. схему выше). Принцип управления змеей будет описан ниже.

Шаг 3: Ползанье

FM2093DJ0X53UMH.MEDIUMF3AN5WFJ140EBRM.MEDIUM

Змеиная чешуя устроена таким образом, что движение в направлении, параллельном положению её тела, вызывает меньшее трение, чем движение в направлении перпендикулярном. В случае роботической змеи подобный эффект достигается благодаря независимой паре колес на каждом из 12 сегментов, которые направлены параллельно длине её тела. Благодаря этому робота можно двигать вперед, просто задав движение по синусоиде или косинусоиде.

Код для Arduino, задающий этот принцип движения, предоставлен по ссылке ниже. Код располагает сервоприводы под определенным углом по отношению друг к другу. Если задать угол в 90 градусов, то змея будет представлять собой прямую линию. Но если угол будет больше 90 градусов, то сервоприводы начнут отклонять прикрепленные к ним сегменты робота влево или вправо. Базовая команда для приведения каждого сервомотора в движение вперед выглядит следующим образом:

sn.write(90+amplitude*cos(frequency*counter*3.14159/180 - n*lag);

В данном случае n – это порядковый номер сегмента, и вместо буквы можно подставить число от 1 до 12. Амплитуда (amplitude) определяет глубину S-образных изгибов, посредством которых движется змея. Частота (frequency) задает скорость передвижения, счетчик (counter) – это переменная цикла, в соответствии с которой происходит волнообразное движение робота, а запаздывание (lag) – постоянная угловая разность между отдельными сегментами. Дробь 3.14159/180 представляет собой лишь соотношение радианов и градусов.

Каждый из сервомоторов управляется подобной командой, но с разными переменными. Все 12 команд объединяются в цикл под названием for, в котором переменная counter идет от 0 до 360 градусов. Одновременное выполнение этих команд приводит к одному полному волнообразному циклу движения змеи вперед, в конце которого она принимает первоначальное положение. Также цикл имеет задержку (delayTime). Она нужна, так как сервоприводы не могут отвечать на команды мгновенно. Для большинства сервоприводов задержки в 7 миллисекунд будет достаточно для плавного перемещения. Если вы захотите, чтобы змея двигалась в обратную сторону, используйте ту же команду, только сделайте так, чтобы счетчик шел от 360 до 0 градусов.

Если синусоида или косинусоида, задающая движение змеи, отцентрирована на 90 градусов, то робот будет двигаться прямо. Но если отцентрировать волну на угол меньше 90 градусов, то змея повернет налево, а если более 90 градусов – то направо. В коде это контролируется переменными leftOffset и rightOffset. Таким образом, чтобы изменить вектор движения змеи, достаточно просто добавить одно из направлений смещения в команды, отвечающие за цикл движения вперед. Но в таком случае змее сперва придется принять изначальное положение, в котором она находилась в начале цикла, и лишь затем повернуть. Чтобы придать повороту плавности, необходимо постепенно довести переменную Offset до необходимого значения, а затем столь же постепенно снизить её до нуля к концу цикла.

Каждая из четырех команд, задающих передвижение, приводит к выполнению одного из волнообразных движений с последующим принятием изначального положения в конце цикла. Для управления роботом используется брелок и приемник, упомянутые в Шаге 1. Каждая из четырех кнопок брелока отвечает за: 1) Движение вперед, 2) Назад, 3) Поворот налево, 4) Поворот направо. Приемник подключается к Arduino следующим образом: контакт, отвечающий за движение вперед, вставляется в разъем 14, назад – в разъем 15, поворот влево – в 16 и вправо в 17. Эти разъемы необходимо назначить разъемами ввода и выставить на LOW. Теперь нужно объединить весь код, отвечающий за движения, в цикл if, который запускается, если соответствующий разъем переключается на HIGH (то есть, при нажатии соответствующей кнопки).

ПОСМОТРЕТЬ КОД

 

/*
Remote control file for serpentine motion
of a snake robot with 12 servos
*/

#include  

// Define servo objects for the snake segments
Servo s1; 
Servo s2;
Servo s3;
Servo s4; 
Servo s5;
Servo s6;
Servo s7;
Servo s8;
Servo s9; 
Servo s10;
Servo s11;
Servo s12;
  
// Define variables
int forwardPin = 14;  // Remote control movement pins
int reversePin = 15;
int rightPin = 16;
int leftPin = 17;

int forwardVal = 0;  // Remote control variables
int reverseVal = 0;
int rightVal = 0;
int leftVal = 0;

int counter = 0; // Loop counter variable
float lag = .5712; // Phase lag between segments
int frequency = 1; // Oscillation frequency of segments.
int amplitude = 40; // Amplitude of the serpentine motion of the snake
int rightOffset = 5; // Right turn offset
int leftOffset = -5; // Left turn offset
int offset = 6; // Variable to correct servos that are not exactly centered
int delayTime = 7; // Delay between limb movements
int startPause = 5000;  // Delay time to position robot
int test = -3; // Test varialble takes values from -6 to +5
  
void setup() 
{ 
// Set movement pins as inputs
  pinMode(forwardPin, INPUT);
  pinMode(reversePin, INPUT);
  pinMode(rightPin, INPUT);
  pinMode(leftPin, INPUT);
  
// Set movement pins to low
  digitalWrite(forwardPin, LOW);
  digitalWrite(reversePin, LOW);
  digitalWrite(rightPin, LOW);
  digitalWrite(leftPin, LOW);
  
// Attach segments to pins  
   s1.attach(2);
   s2.attach(3);
   s3.attach(4);
   s4.attach(5);
   s5.attach(6);
   s6.attach(7);
   s7.attach(8);
   s8.attach(9);
   s9.attach(10);
   s10.attach(11);
   s11.attach(12);
   s12.attach(13);
 
// Put snake in starting position
   s1.write(90+offset+amplitude*cos(5*lag));
   s2.write(90+offset+amplitude*cos(4*lag)); 
   s3.write(90+offset+amplitude*cos(3*lag));
   s4.write(90+amplitude*cos(2*lag));
   s5.write(90+amplitude*cos(1*lag));
   s6.write(90+amplitude*cos(0*lag));
   s7.write(90+amplitude*cos(-1*lag));
   s8.write(90+amplitude*cos(-2*lag));
   s9.write(90+amplitude*cos(-3*lag));
   s10.write(90+amplitude*cos(-4*lag));
   s11.write(90+amplitude*cos(-5*lag));
   s12.write(90+amplitude*cos(-6*lag));
 
   
  delay(startPause);  // Pause to position robot
} 
  
  
void loop() 
{
//  Read movement pins
  forwardVal = digitalRead(forwardPin);
  reverseVal = digitalRead(reversePin);
  rightVal = digitalRead(rightPin);
  leftVal = digitalRead(leftPin);
  
// Forward motion
  if (forwardVal == HIGH){
    for(counter = 0; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }
  }

// Reverse motion
  if (reverseVal == HIGH){
    for(counter = 360; counter > 0; counter -= 1)  {
      delay(delayTime);
      s1.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }    
  }
  
// Right turn
  if (rightVal == HIGH){
// Ramp up turn offset
    for(counter = 0; counter < 10; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }  
// Continue right turn
    for(counter = 11; counter < 350; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }    
// Ramp down turn offset
    for(counter = 350; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    } 
  } 
  
// Left turn
  if (leftVal == HIGH){
// Ramp up turn offset
    for(counter = 0; counter < 10; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }  
// Continue left turn
    for(counter = 11; counter < 350; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }    
// Ramp down turn offset
    for(counter = 350; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    } 
  } 
}

Шаг 4: Движение по прямой линии

F9916R8J0X53UN6.MEDIUM

FT1CD7KJ16OXA9J.MEDIUM

FFQAR4XJ16OXA26.MEDIUM

FSY2DY9J16OXA36.MEDIUM

FQ1WSKCJ16OXA4U.MEDIUM

FGBAP8KJ16OWKPS.MEDIUM

Чтобы заставить змею двигаться по прямой, подобно гусенице, необходимо снять с неё колесики и положить её набок (см. фото выше). Движение происходит посредством направленной в потолок волны, идущей от хвоста к голове. К сожалению, данная конфигурация не предусматривает поворотов. Но на этот случай у нас есть сервопривод постоянного вращения и большое колесико. В то время как первый фрагмент змеи поднят над землей под углом в 90 градусов, два длинных с-образных кронштейна и боковой кронштейн поддерживают сервомотор постоянного вращения и большое колесо, подключенное к нему. Обычно колесо находится параллельно земле, но если змея хочет повернуть, то благодаря расположению сервоприводов колесо переходит в положение, перпендикулярное земле, а сервоприводы в передних сегментах робота отрываются от земли, что переносит вес конструкции на большое колесико. Затем колесо можно повернуть влево или вправо и, таким образом, повернуть всю змею. Когда поворот окончен, колесо отрывается от земли и возвращается в горизонтальное положение, чтобы змея смогла продолжить движение вперед.

Контроль над этим процессом тоже осуществляется с помощью брелока радиоуправления. Его четыре кнопки отвечают за: 1) Движение вперед, 2) Поднятие и опускание колеса, 3) Поворот влево, 4) Поворот вправо. Код команд содержит переменную wheelState, которое может принимать значение 0 или 1. Изначально переменная имеет значение 0. Но при нажатии кнопки 2 переменная превращается в единицу. После нажатия кнопки 2, кнопки 3 и 4 могут быть нажаты для поворота опустившегося на землю колесика. Когда поворот окончен, нажмите кнопку 2 ещё раз. При этом, если wheelState = 1, то колесо оторвется от земли и робот продолжит движение по прямой. И, соответственно, переменная wheelState вновь примет значение 0.

ПОСМОТРЕТЬ КОД

 

/*
Rectilinear motion file for 12 segment snake robot
*/


#include  

// Define servo objects for the snake segments
Servo s1; 
Servo s2;
Servo s3;
Servo s4; 
Servo s5;
Servo s6;
Servo s7;
Servo s8;
Servo s9; 
Servo s10;
Servo s11;
Servo s12;
Servo sWheel;

  
// Define variables
int flex = 45; // Angle to flex a joint
int delayTime = 5; // Delay between limb movements
int startPause = 5000; // Delay to allow robot placement before movement
int pos = 0;  // Loop counter
int smoothnessDelay = 15;  
int wheelState = 0;

int forwardPin =  15;
int wheelTogglePin = 16;
int leftPin = 17;
int rightPin = 18;

int forwardVal = 0;
int wheelToggleVal = 0;
int leftVal = 0;
int rightVal = 0;

  
void setup() 
{ 
// Set movement pins as inputs
  pinMode(forwardPin, INPUT);
  pinMode(wheelTogglePin, INPUT);
  pinMode(rightPin, INPUT);
  pinMode(leftPin, INPUT);
  
// Set movement pins to low
  digitalWrite(forwardPin, LOW);
  digitalWrite(wheelTogglePin, LOW);
  digitalWrite(rightPin, LOW);
  digitalWrite(leftPin, LOW);
  
// Attach segments to PWM pins  
   s1.attach(2);
   s2.attach(3);
   s3.attach(4);
   s4.attach(5);
   s5.attach(6);
   s6.attach(7);
   s7.attach(8);
   s8.attach(9);
   s9.attach(10);
   s10.attach(11);
   s11.attach(12);
   s12.attach(13);
   sWheel.attach(14);

    
// Put snake in straight starting position
   s1.write(0);
   s2.write(90); 
   s3.write(90);
   s4.write(90);
   s5.write(90);
   s6.write(90);
   s7.write(90);
   s8.write(90);
   s9.write(90);
   s10.write(90);
   s11.write(90);
   s12.write(90);
   sWheel.write(90);


// Delay to give time to position robot  
   delay(startPause);
} 
  
  
void loop() 
{
//  Read movement pins
  forwardVal = digitalRead(forwardPin);
  wheelToggleVal = digitalRead(wheelTogglePin);
  rightVal = digitalRead(rightPin);
  leftVal = digitalRead(leftPin);
  
// Forward motion
  if (forwardVal == HIGH){  
    
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s12.write(90-pos);
      s11.write(90+2*pos);
      s10.write(90-pos);
      delay(smoothnessDelay);
    }    

    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s12.write(90-flex+pos);
      s11.write(90+2*flex-3*pos);
      s10.write(90-flex+3*pos);
      s9.write(90-pos);
      delay(smoothnessDelay);
    }    
 
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s11.write(90-flex+pos);
      s10.write(90+2*flex-3*pos);
      s9.write(90-flex+3*pos);
      s8.write(90-pos);
      delay(smoothnessDelay);
    }    
 
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s10.write(90-flex+pos);
      s9.write(90+2*flex-3*pos);
      s8.write(90-flex+3*pos);
      s7.write(90-pos);
      delay(smoothnessDelay);
    }    
 
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {       
      s9.write(90-flex+pos);
      s8.write(90+2*flex-3*pos);
      s7.write(90-flex+3*pos);
      s6.write(90-pos);
      delay(smoothnessDelay);
    }     
  
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s8.write(90-flex+pos);
      s7.write(90+2*flex-3*pos);
      s6.write(90-flex+3*pos);
      s5.write(90-pos);
      delay(smoothnessDelay);
    }
  
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s7.write(90-flex+pos);
      s6.write(90+2*flex-3*pos);
      s5.write(90-flex+3*pos);
      s4.write(90-pos);
      delay(smoothnessDelay);
    }
  
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s6.write(90-flex+pos);
      s5.write(90+2*flex-3*pos);
      s4.write(90-flex+3*pos);
      s3.write(90-pos);
      delay(smoothnessDelay);
    } 
 
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s5.write(90-flex+pos);
      s4.write(90+2*flex-3*pos);
      s3.write(90-flex+3*pos);
      s2.write(90-pos);
      delay(smoothnessDelay);
    } 
 
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    {  
      s4.write(90-flex+pos);
      s3.write(90+2*flex-2*pos);
      s2.write(90-flex+pos);
      delay(smoothnessDelay);
    }    
  } 

// Move wheel up or down
  if (wheelToggleVal == HIGH){
// If wheelState = 0, put wheel into turning position
    if (wheelState == 0){
// Raise wheel
// Bend segment s5 up 45 degrees
// Bend segment s2 down 45 degrees
// Straighten segment s1
      delay(delayTime);
      for(pos = 0; pos < 45; pos +=  1)
      {
        s5.write(90-pos);
        s2.write(90+pos);
        s1.write(2*pos);
        delay(smoothnessDelay);
      }
 // Lower wheel for turning
 // Lower segment s5 by 55 degrees
      delay(delayTime);
      for(pos = 0; pos < 55; pos +=  1)
      {
        s5.write(45+pos);
        delay(smoothnessDelay);
      }   
    }

// If wheelState = 1, put wheel into storage position
    if (wheelState == 1){
// Raise wheel after turning
// Raise segment s5 by 55 degrees
      delay(delayTime);
      for(pos = 0; pos < 55; pos +=  1)
      {
        s5.write(100-pos);
        delay(smoothnessDelay);
      }  
// Bend segment s5 back down 45 degrees
// Bend segment s2 back up
// Bend segment s1 back up
      delay(delayTime);
      for(pos = 0; pos < 45; pos +=  1)
      {
        s5.write(45+pos);
        s2.write(135-pos);
        s1.write(90-2*pos);
        delay(smoothnessDelay);
      } 
    }

    wheelState = 1 - wheelState;
  }

// Left turn
  if (rightVal == HIGH){
    delay(delayTime);
    sWheel.write(80);
    delay(100);
    sWheel.write(90);    
  }

// Right turn
  if (leftVal == HIGH){
    delay(delayTime);
    sWheel.write(100);
    delay(100);
    sWheel.write(90);    
  }

}

Шаг 5: Передвижение боком

FXTLZKCJ0X56S8S.MEDIUM

FO304V6J140EBR7.MEDIUM

FT7OPGFJ140EBR3.MEDIUM

Змеи совершают боковые движения при пересечении неустойчивой местности – например, когда ползут по песку. По большей части, такой способ перемещения представляет собой нечто среднее между прямолинейным и волнообразным движением. И чтобы его достичь, робота необходимо немного переделать. Боковые кронштейны, соединяющие один сегмент с С-образным кронштейном другого, откручиваются и поворачиваются на 90 градусов. Операцию необходимо повторить по длине всей змеи. Таким образом, сервоприводы 1, 3, 5, 7, 9 и 11 будут производить волнообразные движения, а сервоприводы 2, 4, 6, 8, 10 и 12 – прямолинейные. Боковое движение достигается путем задавания движения по горизонтальной косинусоиде нечетным сервоприводам и по вертикальной косинусоиде четным сервоприводам.

ПОСМОТРЕТЬ КОД

 

/*
Sidewind motion for a snake robot with 12 segments
*/

#include  

// Define servo objects for the snake segments
Servo s1; 
Servo s2;
Servo s3;
Servo s4; 
Servo s5;
Servo s6;
Servo s7;
Servo s8;
Servo s9; 
Servo s10;
Servo s11;
Servo s12;
  
// Define variables
int counter = 0; // Loop counter variable
float lag = 1.05; // Phase lag between segments
int frequency = 1; // Oscillation frequency of segments.
int ampHor = 40; // Horizontal amplitude of the sidewind motion of the snake
int ampVert = 40; // Vertical amplitude of sidewind motion of the snake
int offset = 0;
int delayTime = 7; // Delay between limb movements
int startPause = 5000;  // Delay time to position robot
  
void setup() 
{ 
// Attach segments to pins  
   s1.attach(2);
   s2.attach(3);
   s3.attach(4);
   s4.attach(5);
   s5.attach(6);
   s6.attach(7);
   s7.attach(8);
   s8.attach(9);
   s9.attach(10);
   s10.attach(11);
   s11.attach(12);
   s12.attach(13);
 
//Put snake in starting position
   s1.write(90+ampHor*cos(0*lag));
   s3.write(90+ampHor*cos(1*lag)); 
   s5.write(90+ampHor*cos(2*lag));
   s7.write(90+ampHor*cos(3*lag));
   s9.write(90+ampHor*cos(4*lag));
   s11.write(90+ampHor*cos(5*lag));
   
   s2.write(90+ampVert*cos(0*lag));
   s4.write(90+ampVert*cos(1*lag));
   s6.write(90+ampVert*cos(2*lag));
   s8.write(90+ampVert*cos(3*lag));
   s10.write(90+ampVert*cos(4*lag));
   s12.write(90+ampVert*cos(5*lag));
 
   delay(startPause);  // Pause to position robot
} 

  
void loop() 
{
// Serpentine motion
  for(counter = 0; counter < 360; counter += 1)  {
    delay(delayTime);
    s1.write(90+offset+ampHor*cos(frequency*counter*3.14159/180-0*lag));
    s3.write(90+offset+ampHor*cos(frequency*counter*3.14159/180-1*lag));
    s5.write(90+offset+ampHor*cos(frequency*counter*3.14159/180-2*lag));
    s7.write(90+offset+ampHor*cos(frequency*counter*3.14159/180-3*lag));
    s9.write(90+offset+ampHor*cos(frequency*counter*3.14159/180-4*lag));
    s11.write(90+offset+ampHor*cos(frequency*counter*3.14159/180-5*lag)); 
    
    s2.write(90+offset+ampVert*cos(frequency*counter*3.14159/180-0*lag));
    s4.write(90+offset+ampVert*cos(frequency*counter*3.14159/180-1*lag));    
    s6.write(90+offset+ampVert*cos(frequency*counter*3.14159/180-2*lag));
    s8.write(90+offset+ampVert*cos(frequency*counter*3.14159/180-3*lag));
    s10.write(90+offset+ampVert*cos(frequency*counter*3.14159/180-4*lag));    
    s12.write(90+offset+ampVert*cos(frequency*counter*3.14159/180-5*lag));     
  }  
}

Шаг 6: Движение перекатами

FPVA0RKJ0X53UR9.MEDIUM

Ещё один способ передвижения, доступный для этого робота (но не для настоящих змей) – это перекаты. Сперва необходимо собрать змею в вариации для прямолинейного движения, а затем соединить первый и последний сегменты таким образом, чтобы получилось замкнутое кольцо. Но чтобы колесо не падало при движении, необходимо снять голову и хвост с чипом и батарейками. Таким образом, при такой конфигурации движение возможно лишь при удаленной связи робота с Arduino.

/*
Rectilinear motion file for snake robot with 12 segments
*/


#include  

// Define servo objects for the snake segments
Servo s1; 
Servo s2;
Servo s3;
Servo s4; 
Servo s5;
Servo s6;
Servo s7;
Servo s8;
Servo s9; 
Servo s10;
Servo s11;
Servo s12;

  
// Define variables
int flex = 45; // Angle to flex a joint
int delayTime = 5; // Delay between limb movements
int startPause = 5000; // Delay to allow robot placement before movement
int pos = 0;  // Loop counter
int smoothnessDelay = 15; 
int rollState = 0;

  
void setup() 
{ 
  
// Attach segments to PWM pins  
   s1.attach(2);
   s2.attach(3);
   s3.attach(4);
   s4.attach(5);
   s5.attach(6);
   s6.attach(7);
   s7.attach(8);
   s8.attach(9);
   s9.attach(10);
   s10.attach(11);
   s11.attach(12);
   s12.attach(13);

    
// Put snake in straight starting position
   s1.write(90);
   s2.write(90); 
   s3.write(90);
   s4.write(90);
   s5.write(90);
   s6.write(90);
   s7.write(90);
   s8.write(90);
   s9.write(90);
   s10.write(90);
   s11.write(90);
   s12.write(90);


// Delay to give time to position robot  
   delay(startPause);
} 
  
  
void loop() 
{ 
// If snake is straight, put it into rolling position
  if (rollState == 0)
  {
    for(pos = 0; pos < flex; pos +=  1)
    {
      s1.write(90);
      s2.write(90); 
      s3.write(90-pos);
      s4.write(90-2*pos);
      s5.write(90-pos);
      s6.write(90);
      s7.write(90);
      s8.write(90);
      s9.write(90-pos);
      s10.write(90-2*pos);
      s11.write(90-pos);
      s12.write(90);
      delay(smoothnessDelay);
    }
    rollState = 1-rollState;
    delay(1000);
  }    
  
  
// Step 1  
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s12.write(90-pos); 
      s11.write(90-flex-pos);
      s10.write(90-2*flex+pos);
      s9.write(90-flex+pos);
      s6.write(90-pos);
      s5.write(90-flex-pos);
      s4.write(90-2*flex+pos);
      s3.write(90-flex+pos);
      delay(smoothnessDelay);
    }    
// Step 2    
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s1.write(90-pos); 
      s12.write(90-flex-pos);
      s11.write(90-2*flex+pos);
      s10.write(90-flex+pos);
      s7.write(90-pos);
      s6.write(90-flex-pos);
      s5.write(90-2*flex+pos);
      s4.write(90-flex+pos);
      delay(smoothnessDelay);
    }    
// Step 3
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s2.write(90-pos); 
      s1.write(90-flex-pos);
      s12.write(90-2*flex+pos);
      s11.write(90-flex+pos);
      s8.write(90-pos);
      s7.write(90-flex-pos);
      s6.write(90-2*flex+pos);
      s5.write(90-flex+pos);
      delay(smoothnessDelay);
    }    
// Step 4
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s3.write(90-pos); 
      s2.write(90-flex-pos);
      s1.write(90-2*flex+pos);
      s12.write(90-flex+pos);
      s9.write(90-pos);
      s8.write(90-flex-pos);
      s7.write(90-2*flex+pos);
      s6.write(90-flex+pos);
      delay(smoothnessDelay);
    }    
// Step 5
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s4.write(90-pos); 
      s3.write(90-flex-pos);
      s2.write(90-2*flex+pos);
      s1.write(90-flex+pos);
      s10.write(90-pos);
      s9.write(90-flex-pos);
      s8.write(90-2*flex+pos);
      s7.write(90-flex+pos);
      delay(smoothnessDelay);
    }    
// Step 6
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s5.write(90-pos); 
      s4.write(90-flex-pos);
      s3.write(90-2*flex+pos);
      s2.write(90-flex+pos);
      s11.write(90-pos);
      s10.write(90-flex-pos);
      s9.write(90-2*flex+pos);
      s8.write(90-flex+pos);
      delay(smoothnessDelay);
    }   
// Step 7
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s6.write(90-pos); 
      s5.write(90-flex-pos);
      s4.write(90-2*flex+pos);
      s3.write(90-flex+pos);
      s12.write(90-pos);
      s11.write(90-flex-pos);
      s10.write(90-2*flex+pos);
      s9.write(90-flex+pos);
      delay(smoothnessDelay);
    }     
// Step 8
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s7.write(90-pos); 
      s6.write(90-flex-pos);
      s5.write(90-2*flex+pos);
      s4.write(90-flex+pos);
      s1.write(90-pos);
      s12.write(90-flex-pos);
      s11.write(90-2*flex+pos);
      s10.write(90-flex+pos);
      delay(smoothnessDelay);
    }    
 // Step 9
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s8.write(90-pos); 
      s7.write(90-flex-pos);
      s6.write(90-2*flex+pos);
      s5.write(90-flex+pos);
      s2.write(90-pos);
      s1.write(90-flex-pos);
      s12.write(90-2*flex+pos);
      s11.write(90-flex+pos);
      delay(smoothnessDelay);
    }   
 // Step 10
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s9.write(90-pos); 
      s8.write(90-flex-pos);
      s7.write(90-2*flex+pos);
      s6.write(90-flex+pos);
      s3.write(90-pos);
      s2.write(90-flex-pos);
      s1.write(90-2*flex+pos);
      s12.write(90-flex+pos);
      delay(smoothnessDelay);
    }   
 // Step 11
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s10.write(90-pos); 
      s9.write(90-flex-pos);
      s8.write(90-2*flex+pos);
      s7.write(90-flex+pos);
      s4.write(90-pos);
      s3.write(90-flex-pos);
      s2.write(90-2*flex+pos);
      s1.write(90-flex+pos);
      delay(smoothnessDelay);
    }   
 // Step 12
    delay(delayTime);
    for(pos = 0; pos < flex; pos +=  1)
    { 
      s11.write(90-pos); 
      s10.write(90-flex-pos);
      s9.write(90-2*flex+pos);
      s8.write(90-flex+pos);
      s5.write(90-pos);
      s4.write(90-flex-pos);
      s3.write(90-2*flex+pos);
      s2.write(90-flex+pos);
      delay(smoothnessDelay);
    }     
}

Шаг 7: Автономное передвижение

FRZEIQ8J0X56TGN.MEDIUM

Для этого и нужен инфра-красный сенсор, подключенный к микро-сервоприводу, находящемуся в голове змеи. Лучше всего этот способ работает при волнообразном движении. При установленном сенсоре, змея сделает один полный цикл волнообразного движения, остановится, оценит расстояние до ближайших объектов, и если путь чист, то продолжит ползти вперед. При опасном приближении к какому-либо препятствию, микро-сервопривод повернет ИК сенсор направо и налево, оценив расстояние до объектов с обеих сторон. Затем змея повернет в направлении, где препятствий обнаружено меньше, и продолжит идти вперед.

ПОСМОТРЕТЬ КОД

 

/*
Autonomous movement file for robot snake with 12 servos
*/

#include  

// Define servo objects for the snake segments
Servo s1; 
Servo s2;
Servo s3;
Servo s4; 
Servo s5;
Servo s6;
Servo s7;
Servo s8;
Servo s9; 
Servo s10;
Servo s11;
Servo s12;
Servo panServo;
  
// Define variables
int IRpin = 15;
int distanceReading;
int wallDistance;
int wallDistanceTolerance = 30;
int distanceReadingLeft;
int distanceReadingRight;
int wallDistanceLeft;
int wallDistanceRight;
int panDelay = 1000;

int forwardVal = 0;  // Remote control variables
int reverseVal = 0;
int rightVal = 0;
int leftVal = 0;

int counter = 0; // Loop counter variable
float lag = .5712; // Phase lag between segments
int frequency = 1; // Oscillation frequency of segments.
int amplitude = 40; // Amplitude of the serpentine motion of the snake
int rightOffset = 5; // Right turn offset
int leftOffset = -5; // Left turn offset
int offset = 6; // Variable to correct servos that are not exactly centered
int delayTime = 3; // Delay between limb movements
int startPause = 5000;  // Delay time to position robot
int test = -3; // Test varialble takes values from -6 to +5
  
void setup() 
{ 

  
// Attach segments to pins  
   s1.attach(2);
   s2.attach(3);
   s3.attach(4);
   s4.attach(5);
   s5.attach(6);
   s6.attach(7);
   s7.attach(8);
   s8.attach(9);
   s9.attach(10);
   s10.attach(11);
   s11.attach(12);
   s12.attach(13);
   panServo.attach(14);
 
// Put snake in starting position
   s1.write(90+offset+amplitude*cos(5*lag));
   s2.write(90+offset+amplitude*cos(4*lag)); 
   s3.write(90+offset+amplitude*cos(3*lag));
   s4.write(90+amplitude*cos(2*lag));
   s5.write(90+amplitude*cos(1*lag));
   s6.write(90+amplitude*cos(0*lag));
   s7.write(90+amplitude*cos(-1*lag));
   s8.write(90+amplitude*cos(-2*lag));
   s9.write(90+amplitude*cos(-3*lag));
   s10.write(90+amplitude*cos(-4*lag));
   s11.write(90+amplitude*cos(-5*lag));
   s12.write(90+amplitude*cos(-6*lag));
 
   
  delay(startPause);  // Pause to position robot
} 
  
void loop() {
// Point distance sensor straight ahead
  panServo.write(90);
// Take reading from distance sensor
  delay(1000);
  distanceReading = analogRead(IRpin);
  wallDistance = 40-distanceReading/10;
  
// If wallDistance > wallDistanceTolerance, move forward
  if (wallDistance > wallDistanceTolerance) {  
    for(counter = 0; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }
  }

// If wallDistance , wallDistanceTolerance, stop and take
// distance measurements to the left and right
  if (wallDistance < wallDistanceTolerance) {
    panServo.write(170);
    delay(panDelay);
    distanceReadingLeft = analogRead(IRpin);
    delay(panDelay);
    wallDistanceLeft = 40-distanceReadingLeft/10;

    panServo.write(20);
    delay(panDelay);
    distanceReadingRight = analogRead(IRpin);
    delay(panDelay);
    wallDistanceRight = 40-distanceReadingRight/10;
    panServo.write(90);
    delay(panDelay);



    if (wallDistanceLeft > wallDistanceRight) {
// Reverse and then turn left
// Reverse
    for(counter = 360; counter > 0; counter -= 1)  {
      delay(delayTime);
      s1.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }    
  
// Left turn
// Ramp up turn offset
    for(counter = 0; counter < 10; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }  
// Continue left turn
    for(counter = 11; counter < 350; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }    
// Ramp down turn offset
    for(counter = 350; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    } 
    }


    if (wallDistanceLeft <= wallDistanceRight) {
  
// Reverse and turn right
// Reverse
    for(counter = 360; counter > 0; counter -= 1)  {
      delay(delayTime);
      s1.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }    
// Turn right
// Ramp up turn offset
    for(counter = 0; counter < 10; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }  
// Continue right turn
    for(counter = 11; counter < 350; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    }    
// Ramp down turn offset
    for(counter = 350; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+offset+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+offset+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+offset+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      s11.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-5*lag));    
      s12.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-6*lag));     
    } 
    }   
  
  }
   
}

Оригинал статьи

Author

Bender

Я поделюсь с тобой всеми знаниями, которые доступны мне.

Комментарии (0)

There are no comments posted here yet

Оставьте свой комментарий

  1. Posting comment as a guest. Sign up or login to your account.
Вложения (0 / 3)
Share Your Location

О нас

Основой деятельностью портала является показ и объяснение что представляет собой выражени "Робот", "Робототехника", "Законы робототехники", "Мехатроника", "Искусственный интеллект(ИИ)". 

 Если у Вас есть интересная информация по тематике сайта и Вы готовы ей поделиться, - обращайтесь на емайл через форму обратной связи. И мы опубликуем Вашу статью