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
Будущее за бионическими роботами?

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

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

если у вас черное окно попробуйте так. #include <opencv2/opencv.hpp> #include<iostream> #include <stdlib.h> #include <stdio.h> using namespace cv; using namespace std;…

Для Python 3.4: pip3 install tensorflow-1.1.0-cp34-cp34m-linux_armv7l.whl https://github.com/samjabrahams/tensorflow-on-raspberry-pi

PORTB = (PORTB & 0b11100111) | (i & 0b00000011); А почему Вы обнуляете биты 3 и 4, а заполняете биты…

RoboCraft

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

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