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
Будущее за бионическими роботами?
  • 0
  • 19 октября 2020, 07:45
  • admin

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

RSS свернуть / развернуть

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.