Inchworm Robot — робо-червяк на Arduino


Inchworm Robot — робот на Arduino Mini, двигающийся подобно червяку.

Робот приводится в движение пятью сервомашинками MG90S и двумя соленоидными электромагнитами (управляются через ULN2803). А управляется робот через Bluetooth-модуль HC-05.

Скетч для ардуино — Inchworm.ino

#include <Servo.h>
#include <SoftwareSerial.h>
#include <EEPROM.h>

SoftwareSerial mySerial(11, 12); // RX, TX

const int Tailmagnet = 8;
const int Headmagnet = 9;

Servo Stail; //tail servo
Servo Stail2;
Servo Scenter; // center servo
Servo Shead2;
Servo Shead; //head servo
boolean Headmagnetstatus;
boolean Tailmagnetstatus;

String recstring ="";

int Ptail;
int Ptail2;
int Pcenter;
int Phead2;
int Phead;

int Ctail;
int Ctail2;
int Ccenter;
int Chead2;
int Chead;

int Tailstep;
int Tail2step;
int Centerstep;
int Head2step;
int Headstep;

int maxstep=0;
int smoothdelay=60;
int smoothdelayaddr = 0;

int magnetdelay=200;

String movetype;


int pos;
void setup() {
  Stail.attach(2);
  Stail2.attach(3);
  Scenter.attach(4);
  Shead2.attach(5);
  Shead.attach(6);
  pinMode(Headmagnet, OUTPUT);
  pinMode(Tailmagnet, OUTPUT);
  Headmagnetstatus=true;
  Tailmagnetstatus=true;
  delay(1000);

  smoothdelay = EEPROM.read(smoothdelayaddr);
  if(smoothdelay>60)
  {
    smoothdelay=60;
  }else if(smoothdelay<20)
  {
    smoothdelay=20;
  }

  digitalWrite(Headmagnet, LOW);
  digitalWrite(Tailmagnet, LOW);

  Ctail=35;
  Ctail2=125;
  Ccenter=90;
  Chead2=125;
  Chead=35;

  Ptail=Ctail;
  Ptail2=Ctail2;
  Pcenter=Ccenter;
  Phead2=Chead2;
  Phead=Chead;

  Stail.write(Ptail);
  Stail2.write(Ptail2);
  Scenter.write(Pcenter);
  Shead2.write(Phead2);
  Shead.write(Phead);
  mySerial.begin(9600);
  Serial.begin(9600);
}

void loop() {
  recstring="";
  while (mySerial.available()) {
    char inChar = (char)mySerial.read();
    recstring=inChar;
  }
//        if(recstring!="")
//        {
//          Serial.println(recstring);
//        }
        if (recstring=="F")   //front move
        {
          movefront();
        }else if (recstring=="B") //Backmove move
        {
          moveback();
        }else if (recstring=="L") //Left
        {
          movetype="L";
          movefrontturn();
        }else if (recstring=="I") //Left front
        {
          movetype="LF";
          movefrontturn();
        }else if (recstring=="J") //Right front
        {
          movetype="RB";
          movefrontturn();
        }else if (recstring=="R") //Right
        {
          movetype="R";
          movefrontturn();
        }else if (recstring=="K") //Right Back
        {
          movetype="RB";
          movebackturn();
        }else if (recstring=="H") //Left back
        {
          movetype="LB";
          movebackturn();
          recstring="";
        }else if (recstring=="D") //speed down
        {
          if (smoothdelay<60)
          {
            smoothdelay=smoothdelay+5;
            EEPROM.write(smoothdelayaddr, smoothdelay);
          }
        }
        else if (recstring=="U") //speed Up
        {
          if (smoothdelay>20)
          {
            smoothdelay=smoothdelay-5;
            EEPROM.write(smoothdelayaddr, smoothdelay);
          }
        }
}
void movefrontturn()
{
  digitalWrite(Tailmagnet, HIGH);
  delay(magnetdelay);
//  Ctail=45;
//  smoothmove();

  Ctail=80;  //135
  Ctail2=80;
  Ccenter=90;
  Chead2=80;
  Chead=80;
  smoothmove();

//  Ctail=80;
//  smoothmove();

  //digitalWrite(Tailmagnet, LOW);


  if (movetype=="L" ){
    Ccenter=0;
  }else if (movetype=="R" ){
    Ccenter=180;
  }else if (movetype=="LF" ){
    Ccenter=45;
  }else if (movetype=="RF" ){
    Ccenter=135;
  }
  smoothmove();

  digitalWrite(Tailmagnet, LOW);
  delay(magnetdelay);

  digitalWrite(Headmagnet, HIGH);
  delay(magnetdelay);

  Ccenter=90;
  smoothmove();

  digitalWrite(Headmagnet, LOW);
  delay(magnetdelay);


  digitalWrite(Tailmagnet, HIGH);
  delay(magnetdelay);

//  Ctail=900;
  smoothmove();

  Ctail=35; //35
  Ctail2=125;
  Ccenter=90;
  Chead2=125;
  Chead=35;
  smoothmove();

//  Ctail=35; //35
//  smoothmove();

  digitalWrite(Tailmagnet, LOW);
  delay(magnetdelay);
}

void movebackturn()
{
  digitalWrite(Headmagnet, HIGH);
  delay(magnetdelay);
//  Chead=45;
//  smoothmove();

  Ctail=80;
  Ctail2=80;
  Ccenter=90;
  Chead2=80;
  Chead=80;  //135
  smoothmove();

//  Chead=80;
//  smoothmove();

  //digitalWrite(Headmagnet, LOW);

  if (movetype=="LB" ){
    Ccenter=45;
  }else if (movetype=="RB" ){
    Ccenter=135;
  }
  smoothmove();

  digitalWrite(Headmagnet, LOW);
  delay(magnetdelay);

  digitalWrite(Tailmagnet, HIGH);
  delay(magnetdelay);

  Ccenter=90;
  smoothmove();

  digitalWrite(Tailmagnet, LOW);
  delay(magnetdelay);


  digitalWrite(Headmagnet, HIGH);
  delay(magnetdelay);

//  Chead=90;
//  smoothmove();

  Ctail=35;
  Ctail2=125;
  Ccenter=90;
  Chead2=125;
  Chead=35;  //35
  smoothmove();

//  Chead=35; //35
//  smoothmove();

  digitalWrite(Headmagnet, LOW);
  delay(magnetdelay);
}

void movefront()
{
  digitalWrite(Headmagnet, HIGH);
  delay(magnetdelay);
//  Chead=45;
//  smoothmove();

  Ctail=135;
  Ctail2=25;
  Ccenter=90;
  Chead2=25;
  Chead=135;  //135
  smoothmove();

//  Chead=135;
//  smoothmove();

  digitalWrite(Headmagnet, LOW);
  digitalWrite(Tailmagnet, HIGH);
  delay(magnetdelay);

//  Ctail=145;
//  smoothmove();

  Ctail=35; //35
  Ctail2=125;
  Ccenter=90;
  Chead2=125;
  Chead=35;
  smoothmove();

//  Ctail=35; //35
//  smoothmove();

  digitalWrite(Tailmagnet, LOW);
  delay(magnetdelay);
}

void moveback()
{
  digitalWrite(Tailmagnet, HIGH);
  delay(magnetdelay);
//  Ctail=45;
//  smoothmove();

  Ctail=135;  //135
  Ctail2=25;
  Ccenter=90;
  Chead2=25;
  Chead=135;
  smoothmove();

//  Ctail=135;
//  smoothmove();

  digitalWrite(Tailmagnet, LOW);
  digitalWrite(Headmagnet, HIGH);
  delay(magnetdelay);

//  Chead=145;
//  smoothmove();

  Ctail=35;
  Ctail2=125;
  Ccenter=90;
  Chead2=125;
  Chead=35;  //35
  smoothmove();

//  Chead=35; //35
//  smoothmove();

  digitalWrite(Headmagnet, LOW);
  delay(magnetdelay);
}

void smoothmove()
{
  maxstep =0;
  if(abs(Ptail-Ctail)>maxstep)
  {
    maxstep=abs(Ptail-Ctail);
  }
  if(abs(Ptail2-Ctail2)>maxstep)
  {
    maxstep=abs(Ptail2-Ctail2);
  }
  if(abs(Pcenter-Ccenter)>maxstep)
  {
    maxstep=abs(Pcenter-Ccenter);
  }
  if(abs(Phead2-Chead2)>maxstep)
  {
    maxstep=abs(Phead2-Chead2);
  }
  if(abs(Phead-Chead)>maxstep)
  {
    maxstep=abs(Phead-Chead);
  }

  if (maxstep>0)
  {
    Tailstep = (Ctail-Ptail)/maxstep;
    Tail2step = (Ctail2-Ptail2)/maxstep;
    Centerstep = (Ccenter-Pcenter)/maxstep;
    Head2step = (Chead2-Phead2)/maxstep;
    Headstep = (Chead-Phead)/maxstep;
    for (int i = 0; i <= maxstep; i += 1) {
      Ptail=Ptail+Tailstep;
      Ptail2=Ptail2+Tail2step;
      Pcenter=Pcenter+Centerstep;
      Phead2=Phead2+Head2step;
      Phead=Phead+Headstep;
      Stail.write(Ptail);
      Stail2.write(Ptail2);
      Scenter.write(Pcenter);
      Shead2.write(Phead2);
      Shead.write(Phead);

//      Serial.print(Ptail);
//      Serial.print("**");
//      Serial.print(Ptail2);
//      Serial.print("**");
//      Serial.print(Pcenter);
//      Serial.print("**");
//      Serial.print(Phead2);
//      Serial.print("**");
//      Serial.println(Phead);
        delay(smoothdelay);
    }
  }
  Ptail=Ctail;
  Ptail2=Ctail2;
  Pcenter=Ccenter;
  Phead2=Chead2;
  Phead=Chead;
  Stail.write(Ptail);
  Stail2.write(Ptail2);
  Scenter.write(Pcenter);
  Shead2.write(Phead2);
  Shead.write(Phead);
//  Serial.print(Ptail);
//  Serial.print("##");
//  Serial.print(Ptail2);
//  Serial.print("##");
//  Serial.print(Pcenter);
//  Serial.print("##");
//  Serial.print(Phead2);
//  Serial.print("##");
//  Serial.println(Phead);
}

Ссылки
Inchworm Robot - Modular, Move Allsides With BT App

По теме
Sneaky - интерактивный змееподобный робот
Робот-червяк на базе Arduino
Гусеницеподобный робот на Arduino Nano
mOLOID - необычный робот-питомец на Arduino
Будущее за бионическими роботами?


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

Arduino

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

Разделы

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

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

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

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