Робот-змея из Arduino и 12 сервомашинок


Для сборки подобного робота-змеи, автору понадобились: 12 сервомашинок с кронштейнами для крепления, контроллер Arduino Mega и колёсики от конструктора LEGO.

Скетч змееобразного движения:

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

#include <Servo.h>

// 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));
    }
  }
}

Ссылки
Snake Robot

По теме
ChainFORM - роботизированный интерфейс в виде цепочки сервомашинок
Sneaky - интерактивный змееподобный робот
Прототип роботизированной зарядки-змеи от Tesla Motors
Робозмея ACM-R5H
Робот-червяк на базе Arduino
Будущее за бионическими роботами?

Arduino
Arduino, термины, начало работы
КМБ для начинающих ардуинщиков
Состав стартера (точка входа для начинающих ардуинщиков)


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

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна конкурс манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение